Image display apparatus, method and program based on rigid body dynamics

ABSTRACT

An image display system and method that use physical models to produce a realistic display of a scene are disclosed. The image display method involves operating a computer having a display screen, a memory and a processing unit for simulating the motion of objects and displaying the results on the display screen. The method includes the steps of storing in the memory position and velocity parameters which define an initial state of a model system having a plurality of bodies, storing in the memory parameters which define at least one constraint function constraining the motion of the bodies in the model system, and calculating in the processor the position and velocity parameters which define the state of the system after a predetermined time step based on rigid body dynamics, including carrying out a semi-implicit integration step subject to the constraints, to determine the velocity after the step, including determining the constraint forces that act to keep the system in compliance with the constraints by ensuring that the first derivative of the constraint function is zero.

BACKGROUND OF THE INVENTION

The invention relates to an image display system and method, and inparticular to an image display system that uses physical dynamic modelsto produce a realistic display of a scene.

It is becoming increasingly useful to display scenes on computerdisplays that represent the real world. Such scenes may occur in virtualreality devices, simulators and computer games.

One way of providing such scenes is to film images and to display therecorded images on the display. However, this approach requires that thecontent of the scene is predetermined and appropriate film sequencescreated and pre-stored in the computer. Thus, such an approach cannot beused where the scenes are not wholly scripted, which makes the approachunsuitable for simulations and computer games in which the user cancarry out actions not predicted by the programmer.

An alternative approach uses a simulation of rigid body dynamics toallow scenes including such objects to be displayed realistically. Inorder to cope with simulation applications, the model has to be able tocope with a variable number of simulated objects that can be created anddestroyed.

Such models should model a plurality of rigid objects that can interactwith each other, subject to constraints. For example, if one object ishinged to another object that hinge acts as a constraint; the twoobjects cannot move independently. The existence of constraints makesthe problem much more difficult to solve than a simple application ofNewton's laws of motion.

A number of prior approaches have been presented but these have notproven wholly successful. The most suitable for simulation of multipleobjects are so-called “extended coordinate methods” in which theconstraints are introduced using Lagrange multipliers that correspond toforces that maintain the constraints. However, there are difficultieswith these approaches.

Firstly, the known methods use a large number of variables, using nearlydoubling the number of variables (because of the Lagrange multipliers)to describe the system, which results in them being eight times morecomputationally intensive than an equivalent system without constraints.Thus, the prior art methods tend to be highly inefficient.

Secondly, the known methods use differential algebraic equations thatare numerically rather stiff. Simple methods for solving such equationsare rather unstable.

Thirdly, it is not known how to efficiently incorporate friction intosuch systems. As will be appreciated, friction is an important propertyof real physical systems that has to be modelled correctly for arealistic result. This is a difficult problem but a working solution wasreported in D. E. Stewart and J. C. Trinkle, “An implicit time-steppingscheme for rigid body dynamics with inelastic collisions and coulombfriction”, International for numerical methods in engineering, volume 39pages 2673-2691 (1996), and was improved on by Mihai Anitescu and F. A.Potra, “Formulating dynamic multi-rigid-body contact problems withfriction as solvable linear complementarity problems”, Non-linearDynamics, volume 14 pages 231-237 (1997). The approach described allowsconsistent models in which the velocities can always be computed and arealways finite. The disadvantage of the approach is that the modelinvolves solving a particular class of linear complementarity problemwhich has a structure such that not all algorithms are suitable.Anitescu and Trinkle used the Lemke algorithm but this is inefficientand prone to large errors.

A fourth difficulty with prior art approaches is that the constraintsare generated automatically; such constraints need not be notindependent of one another which results in the system being degenerate.Geometric analysis software that performs collision detection cannotcheck whether all the constraints are truly independent of each other,and only during simulation can it be determined that some constraintsare redundant. Such degeneracy can cause real problems for thesimulations, especially in the case of collision detection which checksfor proximity of pairs of objects, whereas the constraint degeneracyonly appears at the system level including all the rigid bodies in thesystem.

Fifthly, known systems do not cope well with stiffness, i.e. rigidspring-like systems and compliant elements. The only tractable solutionsignore contact and friction altogether, which makes them unsuitable foranalysis of arbitrary physical systems.

Accordingly, there is a need for an image display system thatameliorates or alleviates some or all of these difficulties.

The known models require the solution of linear complementarityproblems, a particular type of constrained equation. In general, alinear complementarity problem can be put in the form:Mz+q=w  (1)z_(i)≧0 ∀iε{1, 2 . . . n}  (2)w_(i)≧0 ∀iε{1, 2 . . . n}  (3)x_(i)w_(i)=0 ∀iε{1, 2 . . . n}  (4)

where M is an n by n matrix and z and w are real n-dimensional vectors.The problem requires finding the solution of equation (1), i.e. thevalues of z and w, subject to the constraints (2)-(4).

This is fundamentally a combinatorial problems, and solutions generallysearch through two index sets, where each index is in one of the sets.The first set a is a set of active variables for which w_(i)=0 and thesecond set β is a set of free variables for which z_(i)=0. The problemis then partitioned as

$\begin{matrix}{{{\begin{bmatrix}M_{\alpha\alpha} & M_{\alpha\beta} \\M_{\beta\alpha} & M_{\beta\beta}\end{bmatrix}\begin{bmatrix}z_{\alpha} \\0\end{bmatrix}} + \begin{bmatrix}q_{\alpha} \\q_{\beta}\end{bmatrix}} = \begin{bmatrix}0 \\w_{\beta}\end{bmatrix}} & (5)\end{matrix}$where α and β specify indices in the first and second sets respectively.

This is equivalent to the linear algebra problemM _(αα) z _(α) =−q _(α)w _(β) =M _(βα) z _(α) +q  (6)which must be solved for z while w is calculated by substitution. M_(αα)is known as the principal submatrix.

Various implementations are known. They differ in how the two sets arerevised. They use a computed complementarity point which is a vector ssuch that

$\begin{matrix}{s_{i} = \left\{ \begin{matrix}z_{i} & {\forall{i \in \alpha}} \\w_{i} & {\forall{i \in \beta}}\end{matrix} \right.} & (7)\end{matrix}$The methods go through a sequence of sets until a solution is found,i.e. until s_(i)≧0 ∀iε{1, 2 . . . n}

The Murty principal pivoting algorithm is known from Murty and Yu:“Linear Complementarily, Linear and Non Linear programming” available at

www.personal.engin.umich.edu/˜murty/book/LCPbook/index.html,

and also in an earlier edition published by Helderman-Verlag, Heidelberg(1988), the content of which is incorporated into this specification byreference.

The indices are assigned to a set, and i is set to zero. Then, theprincipal submatrix M_(αα) is formed and solved for z using equation(6).

Then s^((i))=z_(α)+w_(β) is calculated, wherew_(β)=M_(βα)z_(α)  (8)If s≧0 then the algorithm has found a solution. Otherwise, the smallestindex j for which the corresponding element of s is found. If this indexis in the first set, the index is moved to the second, otherwise theindex is in the second set in which case it is moved to the first set.The loop parameter i is incremented and the loop restarted until asolution is found.

The method is illustrated in the flow chart of FIG. 1.

The method is stateless and can be started from any initial guess forthe division of the indices into first and second sets. The matrix willwork on any P matrix and in particular on any positive definite matrix.

The method can fail where the matrix is positive semi-definite. Suchmatrices arise in real physical systems, and can be made positivedefinite by adding a small amount to each element along the diagonal.Kostreva in “Generalisation of Murty's direct algorithm to linear andconvex quadratic programming”, Journal of optimisation theory andapplications, Vol. 62 pp 63-76 (1989) demonstrated how to solve suchpositive semi-definite problems. Basically, the problem is solved for aninitial value of ε, ε is then reduced until the solutions converge; ifthe solutions diverge the problem is unfeasible.

BRIEF SUMMARY OF THE INVENTION

According to the invention, there is provided a method, a computerprogram recorded on a data carrier (e.g. a magnetic or optical disc, asolid-state memory device such as a PROM, an EPROM or an EEPROM, acartridge for a gaming console or another device), for controlling acomputer (e.g. a general purpose micro, mini or mainframe computer, agaming console or another device) having a display screen, a memory anda processing unit, the computer program being operable to control thecomputer to carry out the method, and a computer programmed to carry outthe method.

The method according to the invention may include the steps of:

-   -   storing in a memory position and velocity parameters defining an        initial state of a model system having a plurality of bodies,    -   storing in the memory parameters defining at least one        constraint function constraining the motion of the bodies in the        model system, and    -   calculating in the processor the position and velocity        parameters defining the state of the system after a        predetermined time step based on rigid body dynamics, including    -   carrying out a semi-implicit integration step subject to the        constraints, to determine the velocity after the step, including    -   determining the constraint forces that act to keep the system in        compliance with the constraints by ensuring that the first        derivative of the constraint function is zero.

In known approaches, the second derivative was held to be zero. However,the method according to the invention provides much faster calculation.

The method may cause the computer to carry out the further step ofdisplaying an image of the objects at their calculated positions on thecomputer display screen, so that the display shows the objects on thescreen using physical laws to simulate their motion.

The means for determining the constraint forces may include solving thelinear complementarity problem using the Murty algorithm.

The calculating step may include carrying out the implicit integrationby

-   -   calculating the velocity parameters after the time step from the        external forces, the constraint forces and the position and        velocity parameters before the time step, and    -   calculating the position parameters after the time step from the        external forces and constraint forces, the calculated velocity        parameters after the time step and the position parameters        before the time step; and

In prior art image display methods implementing rigid body dynamics theaccelerations have been taken as parameters. In the method according tothe invention, the parameters calculated are position and velocity.

The means for solving the linear complementarity problem may includesolving the boxed LCP problem by the modified Murty's method.

In order to implement maximum bounds on the constraint forces thecalculation may include the step of testing whether the constraintforces have a magnitude greater than a predetermined value and if sosetting them to be that predetermined value. This has not previouslybeen done but leads to a more efficient solution.

Preferably, the model includes a model of friction. Static frictionrequires that the tangential force f_(t) of magnitude less than or equalto the static friction coefficient μ_(s) times the normal force f_(n).The force f_(t) due to dynamic friction has a magnitude equal to thedynamic friction coefficient μ_(s) times the normal force, and adirection given by f_(t) v_(t)≦0.

The dependence of the frictional force on the normal force can bereplaced by a novel approximation in which the friction, dynamic orstatic, is not dependent on the normal force. This force thencorresponds to a simple bounded multiplier, i.e. a force that can haveup to a predetermined value. Thus the force exactly fits the model usedin any event in which the constraint forces Lagrange multipliers havemaximum values; friction in the model is equivalent to another boundedconstraint force. Thus making this approximation substantiallysimplifies the inclusion of friction in the model.

Accordingly, the method may include a model of friction in which thefrictional force between a pair of objects is independent of the normalforce between the objects.

The frictional force between each pair of objects may be modelled as abounded constraint force in which the constraint force acts in the planeof contact between the pair of objects to prevent sliding of one of thepair of objects over the other of the pair, wherein the constraint forceis bounded to be not greater than a predetermined constant value toallow sliding of the objects over one another and thus include dynamicfriction in the simulation.

In order to implement bounded constraint forces, the method may includethe step of testing whether the constraint forces have a magnitudegreater than a predetermined value and if so setting them to be thatpredetermined value.

The method may include a relaxation parameter y introduced to determinehow exactly to satisfy the constraints.

The friction model taken leads to a symmetric positive definite linearcomplementarity problem, in which the only friction conditions aresimple inequalities. This allows the use of the much more efficientMurty algorithm that the less useful Lemke algorithm.

From further aspects, this invention provides a computer program that isa computer game program, which game program may be recorded within acartridge for a computer game machine; and a computer game programmed togenerate a display by means of a computer program according to anypreceding claim.

BRIEF DESCRIPTION OF THE DRAWINGS

Specific embodiments of the invention will now be described, purely byway of example, with reference to the accompanying drawings in which

FIG. 1 shows a flow diagram of a prior art implementation of the Murtyalgorithm,

FIG. 2 shows a computer running the present invention,

FIG. 3 shows a flow diagram of the implementation of the presentinvention,

FIG. 4 shows the friction pyramid,

FIG. 5 shows the box-friction model used to approximate to the frictionpyramid,

FIG. 6 is a flow chart of the method including friction, and

FIG. 7 is a flow chart of the program according to the invention.

FIG. 8 is an illustration of a friction pyramid.

DETAILED DESCRIPTION OF THE INVENTION

The implementation of the invention that will be described includes acomputer system 1 having a display 3, a memory 5 and a processor. Thecomputer system has software 9 loaded into the computer memory to allowthe computer system to display a simulation of the real physical worldon the display 3. The display 3 may be a conventional computer displayscreen, for example an Liquid Crystal Display (LCD) screen or a CathodeRay Tube (CRT) screen, or the display 3 may be a less conventionaldisplay type such as virtual reality goggles or multiple screens of acoin operated video game, of the type installed in public places.

A physical system which has n rigid bodies is modelled. The i^(th) bodyhas a mass m_(i), and a position vector p which has s seven coordinates,three to define the position of the rigid body and four being the Eulerparameters of the body defining its orientation. Each body also hasvelocity vector v which has six coordinates, three being the threecomponents of linear velocity and three being the three components ofangular velocity, each relative to the inertial frame. Further detailsabout the coordinates and the matrices used to convert betweencoordinates in the local reference frame and those in an inertial frameare given in sections 1 to 1.4 of Appendix 1.

The key point to note is that equations 1.36 and 1.37 of Appendix 1define Newton's laws for a dynamical system in a form similar toequation (1), i.e.M{dot over (v)}=f _(e) +f _(c) +f _(r)  (9)where M is the block diagonal matrix defined in equation 1.37 ofappendix 1.

A rigid body dynamics problem then becomes equivalent to solvingequation (7) subject to the dynamic constraints. This is carried out bynumerical integration. The difficult part is calculating the constraintforce.

The initial state of the system is set up in the computer memory, andincludes the above parameters.

Constraints governing the rigid bodies may also be set up. The software9 may include definitions of such constraints. An example of aconstraint is a hinge between two rigid body elements so that theelements cannot move independently, the system contains constraintinitialization routines for setting up such constraints. A separateroutine is provided for each type of constraint; the routine is called,defining the one, two or three constrained elements and otherinformation required to specify the constraint. For example, if thereare two constrained elements in the form of two rigid bodies are joinedby a ball and socket joint, the information required is the identity ofthe constrained elements and the position of the ball and socket joint.The constraint information is stored in the form of the Jacobian ofconstraint.

A simple example of a constraint would be a rigid floor at height zeroin the model. The constraint function Φ(p_(x), p_(y), p_(z)) is thenchosen to be Φ(p)=p_(z), the conventional constraint Φ(p)≧0 then being adefinition of the constraint.

The solution method used requires a Jacobian J of the constraintfunction Φ(p)—this is related to the more conventional Jacobian J_(p) ofthe function Φ(p) with respect to position by J=J_(p)Q where Q isdefined at equation 1.40 of Appendix 1.

The method used does not require storing the second derivative of theconstraint function.

After the initial conditions and constraints are set up, the routine maybe called to simply step forward in time by a predetermined time period.Indeed, the use of this simple program structure in which a routine iscalled with a matrix and outputs a like matrix of the results one timestep forward is a significant advantage over prior approaches in whichthe movement forward in time has not been encapsulated in this way.

The way in which the system moves one step forward is based on simpleEuler integration, i.e. calculating the final positions and velocitiesfrom the initial positions and velocities and the applied forces. Ofcourse, some of the forces are the constraint forces that ensure thatthe system remains in accordance with the constraints; the way these arecalculated will be described below.

Euler integration can be explicit, in which the integration is based onthe values at the start of the step, or implicit in which the values atthe end of the step are used. In the method of the invention, asemi-implicit approach is used in which the positions after the step arecalculated using the positions at the start of the step and thevelocities at the end of the step and the velocities are calculatedexplicitly. Put mathematically, the position p and velocity v at the(i+1)^(th) step after a time h are given byp _(i+1) =p _(i) +hv _(i+1)  (10)v _(i+1) =v _(i) +h·M ⁻¹ ·f  (11)

where M is the block diagonal matrix defined in Appendix 1 and f is thesum of the forces on the system, including the constraint forces tomaintain the constraint. Note that the equations for the positionvariables are implicit and the equation for velocity explicit. Thus, theabove equations need to be solved subject to the constraint equations.

Of course, in order to carry out the above calculation it is necessaryto calculate f. The force f is made up of the external force plus theeffects of the external torques plus the constraint force that keeps thesystem in accordance with the constraints. Thus, the constraint forceson the system must be calculated. Appendix 1 at 1.51 demonstrates how todo this for an explicit Euler scheme to calculate subject to theconstraint Φ(p)=0. In the conventional scheme, the constraints arecalculated by setting the second derivative of Φ to zero.

In the method according to the embodiment, however, this is not done andthe first derivative

$\frac{\partial\Phi}{\partial p}$is set to zero.

The detail is set out in sections 1.5 to 1.6 of Appendix 1. The approachof using the velocity constraints rather than the second derivative ofthe constraint function has both advantages and disadvantage. The keydisadvantage is that although this approach guarantees that the velocityconstraints are satisfied it does not guarantee that the positionconstraints are.

The constraint equation may be given byφ_(o) +J _(p)(p′−p)=0  (12)

where Jp is the Jacobian of the constraint forces based on the position,i.e.

$J_{p} = {\frac{\partial\Phi}{\partial p}.}$This is related to the J actually calculated by J=J_(p)Q.

Rather than satisfy this exactly, the parameter γ is introduced byamending the right hand side of equation (12) so that it readsφ_(o) +J _(p)(P′−p)=(1−γ)φ_(o)  (13).

When γ=1 equation (13) becomes equivalent to equation (12). A value of0.2 has been found suitable.

A solution for the forces is given at 1.5.5. of Appendix 1.

The result is:

$\begin{matrix}{{\left( {{J.M^{- 1}}J^{T}} \right)\lambda} = {\frac{{- \gamma}\;\phi_{0}}{h^{2}} - \frac{Jv}{h} - {{JM}^{- 1}\left( {f_{c} + f_{r}} \right)}}} & (14)\end{matrix}$

This is an equation in the form Ax+b=w and it can be solved by thealgorithm presented below to find the vector γ.

The constraint force fc is then given byf_(c)=J^(T)λ.  (15)

The constraint forces may then be fed into the integration step tocompute the position of each object after the timestep.

It should be noted that the method uses bounded constraints. In otherwords, each element of the force is not allowed to become arbitrarilylarge as in previous methods but is bounded. This is simply implementedby testing to see if the elements of the force are larger than the boundand if so reducing the element to the bound.

The problem posed is not directly amenable to simple solution by theMurty algorithm. Rather, it has the slightly different structure of aboxed linear complementarity problem, as follows:Az+q=w ₊ −w.z _(i)−1_(i)≧0w _(+i)≧0(z _(i)−1_(i))w _(+i)=0(u _(i) −z _(i)≧0w _(−i)≧0(u _(i) −z _(i))w _(−i)=0  (16)

The w₊ and w⁻ terms come from the integration step; they correspond toforces/accelerations from upper and lower boundaries respectively, thepositions of the boundaries being given by u and 1. The z termcorresponds to the differential of the velocity.

The result gives the constraint force which can be plugged into theintegration.

The above formalism is equivalent to a mixed complementarity problemdefined as

$\begin{matrix}{{{\begin{bmatrix}A & {- I} & I \\I & 0 & 0 \\{- I} & 0 & 0\end{bmatrix}\begin{bmatrix}\begin{matrix}z \\w_{+}\end{matrix} \\w_{-}\end{bmatrix}} + \begin{bmatrix}\begin{matrix}q \\{- l}\end{matrix} \\u\end{bmatrix}} = {\begin{bmatrix}\begin{matrix}0 \\\mu\end{matrix} \\v\end{bmatrix}.}} & (17)\end{matrix}$

This can be solved by partitioning the second set defined above into twosets, γ and ι, so that z_(j)=1_(j) for jεy and Zj=Uj for jει.Afterwards, the Murty algorithm is followed with a different definitionof the complementarity point, namely,

$\begin{matrix}{s_{j} = \left\{ \begin{matrix}{\min\left( {{z_{j} - l_{j}},{u_{j} - z_{j}}} \right)} & {\forall{j \in \alpha}} \\\left( {{A_{\gamma\;\alpha}z_{\alpha}} - {A_{\gamma\gamma}l_{\gamma}} - {A_{\gamma\iota}u_{l}} + q_{y}} \right) & {\forall{j \in \gamma}} \\{- \left( {{A_{\iota\;\alpha}z_{\alpha}} - {A_{\iota\gamma}l_{\gamma}} - {A_{\iota\iota}u_{\iota}} + q_{\iota}} \right)} & {\forall{j \in \iota}}\end{matrix} \right.} & (18)\end{matrix}$

The least index rule is applied to the complementarity point as follows:j=min arg(s _(j)<0)If jεα and z_(j)<1_(j) then remove j from α and put it in γIf jεα and z_(j)>u_(j) then remove j from α and put it in ιIf jεγ add j to α and remove it from γIf jε1 add j to α and remove it from ιThe loop is then repeated until there is no element S_(j)<0.

FIG. 3 sets out a flow chart of the Murty algorithm that is used tomodel the constraints. It is based on that of FIG. 1, with amendments tocope with the bounded parameters which are may be used to modelfriction.

This solution will be referred to as the Boxed LCP solution, which hasbeen developed independently by the inventors.

The above solution may be refined by replacing the zeroes with atolerance parameter −ε. This is similar to the Kostreva method, excepthere the parameter is not reduced but simply kept at a constant valuesay 10⁻³.

The parameters γ and ε may be chosen to model a compliant couplingbetween two of the rigid bodies. In this way springs and compliantbodies may be included in the model without any complexity.

Appendix 4 sets out how the parameters ε and γ already present in themodel can be used to model stiff springs. Such parameters are veryuseful for modelling car suspensions, and the like. Thus, the use of themodel provides the unexpected benefit of being usable not merely toensure fitting of the constraints but can also be used to model stiffsprings without any additional parameters or programming.

A key advantage provided by the approach selected is that it allows theinclusion of friction.

The friction constraint can thus be considered to be given by a conewhich; if the contact force between two bodies is given by a forcevector the values of the dynamic friction when the bodies slide is givenby a cone in the three dimensional space of the force (FIG. 4). In theinvention this cone is approximated by a four sided box, in other wordsfriction is approximated by a model in which the transverse frictionalforce is not dependent on the normal force (FIG. 5).

The method thus works as follows:

Step 1: For each contact point, apply a non-penetration constraint and astatic friction constraint.

Step 2: Estimate a normal force λ_(i) for each point

Step 3: At each point of contact, limit the Lagrange multipliers thatenforce zero relative velocity to have a maximum value proportional tothe normal force: i.e.−μλ_(i)≦β_(ij)≦μλ_(i)Step 4: solve with the boxed Murty algorithmStep 5: refine estimate of normal forces and repeat if required.

A flow chart of this procedure is presented in FIG. 6.

Surprisingly, such a crude model still gives good realistic results. Thereal advantage is that the model does not involve a combination of theLagrange multipliers (constraint forces) as a constraint—the constraintsare of the simple form f_(t)≦a constant, whereas for more realisticmodel the upper bound on f_(t) would depend on the normal force. Thisallows the use of a simpler algorithm as set out above. Indeed, thebounded constraint force has exactly the same bounded force as used forall of the constraints on the objects; accordingly adding friction doesnot add significantly to the difficulty of solving the problem.

Thus, the described embodiment allows much faster processing of thesimulation.

A list of the routines used in the program implementing the embodiment,including the many routines to set up individual types of constraint, isprovided in Appendix 2.

The program implementing the above is schematically shown in FIG. 7.

Firstly, a body data array containing the information about each rigidobject, i.e. its position, orientation and mass is initialized. Aconstraint data array is likewise initialized containing parametersdefining the constraints.

In the next step, the constraint data array is interrogated to create alist of Jacobians.

Then, a matrix A is calculated given by A=JM⁻¹J^(T) where M is the massmatrix as defined in Appendix 1 and J is the Jacobian. This step is doneso that A is an upper triangle matrix (and symmetric).

The upper triangle elements of A are then copied to the lower, and thediagonal modified. Rotational force may be added to the bodies at thisstage. The A matrix is then complete.

Next, A is factorized to find A⁻¹ by Cholesky deposition.

An intermediate result rhs is then calculated as follows: Calculation ofrhs, for each body:tmp=0tmp=M ⁻¹ ·f _(e)tmp=tmp+v/hrhs=c/h−γ/h ² −J·tmp

Then, rhs, A, A⁻¹ and the lower and upper constraint vectors l and u aresent to a routine which calculates λ, the Lagrange multipliers, bysolving equation (16) by these parameters by the boxed LCD method asdescribed above.

Next, the resultant forces are calculated from λ and J, and the resultspassed to the semi-implicit integrator.

This outputs the velocity, force, position and orientation, in the formof the transformation matrix and orientation quaternion of each body.

Finally, a screen image is calculated displaying the objects in theirnew locations and this is displayed on the video display unit.

The method described works much faster than previously known methods.Accordingly, it becomes possible to more accurately simulate real timescenes, and provide an improved simulation that more rapidly andaccurately simulates the real physical world.

APPENDIX 1 1 Rigid Body Dynamics 1.1 Definitions

Acronyms

-   -   COM: Center of mass.    -   IF: Inertial frame of reference for the whole system.        Notation α (any 3×1 vector) is relative to the IF.    -   a′ is relative to the coordinate system of some rigid body. a        and a′ are related by a unitary (rotational) transformation.    -   I_(i) is the identity matrix of size i×i.    -   0_(i) is the zero matrix of size i×i.    -   If a and b are 3×1 vectors then        a×b=âb  (1.1)        where

$\begin{matrix}{\hat{a} = {\left. \begin{bmatrix}0 & {- a_{3}} & a_{2} \\a_{3} & 0 & {- a_{1}} \\{- a_{2}} & a_{1} & 0\end{bmatrix}\Rightarrow{\hat{a}a} \right. = 0}} & (1.2) \\{{\frac{\mathbb{d}\;}{\mathbb{d}p}\; F\;(p)} \equiv {{\nabla_{p}\; F}\;(p)}} & (1.3)\end{matrix}$

1.2 Parameters of the Mechanical System

-   -   There are n rigid bodies (“links”) numbered 1 . . . n. The        coordinate system for each body has its origin at its center of        mass.    -   Link i has mass m_(i) and 3×3 body-relative inertia tensor        H_(i).    -   There are m constraint equations.    -   p (7n×1) is the system position vector, which has the structure        p=[p₁p₂ . . . p_(n)]^(T)        where        p_(i)=[x_(i)y_(i)z_(i)q_(i) ¹q_(i) ²q_(i) ³q_(i) ³]^(T)  (1.5)        where (x,y,z) is the position of the center of mass and (q¹, q²,        q³, q⁴) is the orientation quaternion (Euter parameters). These        are both relative to the IF.    -   v (6n×1) is the system velocity vector, which has the structure        v=[v₁v₂ . . . v_(n)]^(T)  (1.6)        where        v_(i)=[{dot over (x)}_(i){dot over (y)}_(i)ż_(i){dot over        (ω)}_(i) ¹{dot over (ω)}_(i) ²{dot over (ω)}_(i) ³]^(T)  (1.7)        where ({dot over (x)}, {dot over (y)}, ż) is the velocity of the        center of mass and (ω¹ ω² ω³) is the angular velocity (both        relative to the IF).        R_(i) is the 3×3 unitary rotation matrix corresponding to the        Euler parameters for body i.

$\begin{matrix}{R = \begin{bmatrix}{{q^{1}\; q^{1}} + {q^{2}\; q^{2}} - {q^{3}\; q^{3}} - {q^{4}\; q^{4}}} & {{2q^{2}\; q^{3}} - {2q^{1}\; q^{4}}} & {{2q^{1}\; q^{3}} + {2q^{2}\; q^{4}}} \\{{2q^{2}\; q^{3}} + {2q^{1}\; q^{4}}} & {{q^{1}\; q^{1}} - {q^{2}\; q^{2}} + {q^{3}\; q^{3}} - {q^{4}\; q^{4}}} & {{{- 2}q^{1}\; q^{2}} + {2q^{3}\; q^{4}}} \\{{{- 2}q^{1}\; q^{3}} + {2q^{2}\; q^{4}}} & {{2q^{1}\; q^{2}} + {2q^{3}\; q^{4}}} & {{q^{1}\; q^{1}} - {q^{2}\; q^{2}} - {q^{3}\; q^{3}} - {q^{4}\; q^{4}}}\end{bmatrix}} & (1.8)\end{matrix}$Force vectors (such as f_(e), the external force) have the structuref=[f₁f₂ . . . f_(n)]^(T)  (1.9)wheref₁=[f_(i) ^(x)f_(i) ^(y)f_(i) ^(z)T_(i) ^(x)T_(i) ^(y)T_(i)^(z)]^(T)  (1.10)where (f^(x), f^(y), f^(z)) is the force applied to center of mass and(T^(x), T^(y), T^(z)) is the applied torque (both relative to the IF).

1.3 Equations of Motion for One Rigid Body

1.3.1 Coordinate Transformation

R is the unitary transformation matrix for link i (3×3). If a is a pointin the IF and a′ is a point in the link frame then a=Ra′. Also note thatR ⁻¹ =R ^(T)  (1.11)The ith column vector of R is u_(i), so that

$\begin{matrix}{R = \left\lbrack {{\overset{.}{\overset{.}{\underset{\underset{.}{.}}{u}}}}_{i}{\overset{.}{\overset{.}{\underset{\underset{.}{.}}{u}}}}_{2}{\overset{.}{\overset{.}{\underset{\underset{.}{.}}{u}}}}_{3}} \right\rbrack} & (1.12)\end{matrix}$The u_(i) vectors are axis unit vectors for the body.1.3.2 Rotating Frame

If a is an IF point in the body relative to the center of mass then{dot over (a)}=ω×a  (1.13)Thus{dot over (R)}=[{dot over (u)}₁{dot over (u)}₂{dot over (u)}₃]  (1.14)=[ω×u _(i) ω×u ₂ ω×u ₃]  (1.15)=[{circumflex over (ω)}u₁{circumflex over (ω)}u₂ω×u₃]  (1.16)={circumflex over (ω)}R  (1.17)Note that {circumflex over (ω)}={circumflex over (ω)}^(T) and {circlearound (ω)}ω=0.1.3.3 Angular MomentumTo start with we will consider just the rotational part of motion, andignore the linear part (which is a lot easier to deal with). Thedefinition of angular momentum L for a rigid body is

$\begin{matrix}{L \equiv {\sum\limits_{i}^{\;}\;{q_{i} \times M_{i}{\overset{.}{q}}_{i}}}} & (1.18)\end{matrix}$where q_(i) are the positions of all particles in the rigid body (w. r.t. the IF) and M_(i) are their masses. Now,

$\begin{matrix}{\overset{.}{L} = {\sum\limits_{i}^{\;}\;{\frac{\mathbb{d}\;}{\mathbb{d}t}\;\left( {q_{i} \times M_{i}\;{\overset{.}{q}}_{i}} \right)}}} & (1.19)\end{matrix}$

$\begin{matrix}{= {{\sum\limits_{i}^{\;}{{\overset{.}{q}}_{i} \times M_{i}\;{\overset{.}{q}}_{i}}} + {\sum\limits_{i}^{\;}\;{q_{i} \times M_{i}\;{\overset{¨}{q}}_{i}}}}} & (1.20) \\{= {\sum\limits_{i}^{\;}\;{q_{i} \times g_{i}}}} & (1.21)\end{matrix}$where g_(i) is the force on particle i. Now,

$g_{i} \equiv {{\sum\limits_{j \neq i}^{\;}\; g_{ij}} + G_{i}}$where g_(ij) is the force between q_(i) and qj, and G_(i) is theexternal force on q_(i). It can be shown that if g_(ij)=−g_(ji) (as isthe case according to Newton) then the total contribution to L from theg_(ij) is zero, so

$\begin{matrix}{\overset{.}{L} \equiv {\sum\limits_{i}^{\;}\;{q_{i} \times G_{i}}}} & (1.22)\end{matrix}$Where the sum over i only includes those particle positions at which anexternal force is acting. Introducing the effect of torque is easy—all“pure” torques applied to the rigid body effectively works through thecenter of mass, so

$\begin{matrix}{\overset{.}{L} = {{\sum\limits_{i}^{\;}\;{q_{i} \times G_{i}}} + {\sum\limits_{i}^{\;}\;\tau_{i}}}} & (1.23)\end{matrix}$The quantity {dot over (L)} is effectively the external rotational forceapplied to the body, which is usually a combination of joint constraintforces and forces external to the rigid body system.1.3.4 The Link Between ω and LIt can be shown thatL′=Hω′  (1.24)where L=RL′, ω=Rω′, and H is the 3×3 inertia tensor for this body. ThusR^(T)L=HR^(T)ω  (1.25)L=RHR^(T)ω  (1.26)from the chain rule

$\begin{matrix}{\overset{.}{L} = {{\frac{\mathbb{d}\;}{\mathbb{d}t}\;\left( {RHR}^{T} \right)\;\omega} + {{RHR}^{T}\;\omega}}} & (1.27)\end{matrix}$=({dot over (R)}HR ^(T) ω+RH{dot over (R)} ^(T) ω+RHR ^(T){dot over(ω)})  (1.28){dot over (R)} ^(T)ω=({circumflex over (ω)}R)^(T)ω  (1.29)=R^(T){circumflex over (ω)}^(T)ω  (1.30)=0  (1.31)soRHR ^(T) {dot over (ω)}={dot over (L)}−{dot over (R)}HR ^(T)ω  (1.32)={dot over (L)}−{circumflex over (ω)}(RHR ^(T))ω  (1.33)={dot over (L)}−ω×((RHR ^(T))ω)  (1.34)or, schematically,(inertia tensor in IF)×(angular acceleration)=(external force)+(coriolisforce)  (1.35)which is simply a statement of Newton's law.

1.4 Equations of Motion for a Rigid Body System

For n rigid bodies we simple combine the equations of motion into onematrix equation:M{dot over (v)}=f _(e) +f _(c) +f _(r)  (1.36)where M is a block diagonal matrix (with 3×3 blocks):

$\begin{matrix}{M = \begin{bmatrix}{m_{1}\; I_{3}} & \; & \; & \; & \; & \; & \; \\\; & {R_{1}\; H_{1}\; R_{1}^{T}} & \; & \; & \; & \; & \; \\\; & \; & {m_{2}\; I_{3}} & \; & \; & \; & \; \\\; & \; & \; & {R_{2}\; H_{2}\; R_{2}^{T}} & \; & \; & \; \\\; & \; & \; & \; & \ldots & \; & \; \\\; & \; & \; & \; & \; & {m_{n}\; I_{3}} & \; \\\; & \; & \; & \; & \; & \; & {R_{n}\; H_{n}\; R_{n}^{T}}\end{bmatrix}} & (1.37)\end{matrix}$and f_(e) is the external force (applied from “outside” the rigid bodysystem), f_(c) is an undetermined constraint force, and f_(r) is therotational force:

$\begin{matrix}{f_{r} = \begin{bmatrix}{{- \omega_{1}} \times \left( {\left( {R_{1}\; H_{1}\; R_{1}^{T}} \right)\;\omega_{1}} \right)} \\\vdots \\{{- \omega_{n}} \times \left( {\left( {R_{n}\; H_{n}\; R_{n}^{T}} \right)\;\omega_{n}} \right)}\end{bmatrix}} & (1.38)\end{matrix}$The state variables of the system are p and v. To integrate the system,the state derivatives must be known. The above equation allows us todetermine {dot over (v)}, assuming that f_(c) is known). {dot over (p)}is determined by applying a transformation that allows the angularvelocity to be converted into Euler parameter derivatives{dot over (p)}=Qv  (1.39)

$\begin{matrix}{Q = \begin{bmatrix}I_{3} & \; & \; & \; & \; & \; & \; \\\; & D_{1} & \; & \; & \; & \; & \; \\\; & \; & I_{3} & \; & \; & \; & \; \\\; & \; & \; & D_{2} & \; & \; & \; \\\; & \; & \; & \; & \ldots & \; & \; \\\; & \; & \; & \; & \; & I_{3} & \; \\\; & \; & \; & \; & \; & \; & D_{n}\end{bmatrix}} & (1.40)\end{matrix}$where

$\begin{matrix}{D_{i} = {0.5\begin{bmatrix}{{- q_{i}^{2}} - q_{i}^{3} - q_{i}^{4}} \\{{+ q_{i}^{1}} + q_{i}^{4} - q_{i}^{3}} \\{{- q_{i}^{4}} - q_{i}^{1} - q_{i}^{2}} \\{{+ q_{i}^{3}} - q_{i}^{2} + q_{i}^{1}}\end{bmatrix}}} & (1.41)\end{matrix}$Note that D_(i) ^(T)D_(i)=I₃/4.

1.5 Integration with Implicit Constraint Projection

1.5.1 Explicit Euler

The explicit Euler integration step isnew p=p*=p+hQv  (1.42)new v=v*=v+hM ⁻¹(f _(e) +f _(c) +f _(r))  (1.43)As the system evolves we want to compute f_(c) such that the followingconstraint is satisfied:φ(p)=0  (1.44)Note that φ(p) has size m×1. Make a first order approximation to this:

$\begin{matrix}{{\phi\;(p)} = {{\phi\;\left( p_{0} \right)} + {\frac{{\partial\phi}\;\left( p_{0} \right)}{\partial p}\;\left( {p - p_{0}} \right)}{\overset{.}{+}}^{-}\ldots}} & (1.45) \\{\approx {\phi_{0} + {J_{p}\;\left( {p - p_{0}} \right)}}} & (1.46)\end{matrix}$where p_(o) is the current position, and J_(p) is the m×7n Jacobian ofthe constraint function with respect to position (evaluated at p₀). Findthe first derivative of φ:

$\begin{matrix}{{\phi\;(p)} = {\frac{\partial\phi}{\partial p} \cdot \frac{\partial p}{\partial t}}} & (1.47) \\{= {J_{p}\;\overset{.}{p}}} & (1.48) \\{= {JpQv}} & (1.49) \\{= {Jv}} & (1.50)\end{matrix}$Where J is the Jacobian of the constraint function with respect tovelocity (J=J_(p)Q). Find the second derivative of φ:

$\begin{matrix}{{\phi\;(p)} = {{\frac{\partial\;}{\partial t}\;{\frac{\partial\phi}{\partial p} \cdot \frac{\partial p}{\partial t}}} + {{\frac{\partial\phi}{\partial p} \cdot \frac{\partial\;}{\partial t}}\;\frac{\partial p}{\partial t}}}} & (1.51) \\{= {{\frac{\partial\;}{\partial p}\;\frac{\partial\phi}{\partial p}\;\overset{.}{p}\overset{.}{p}} + {J_{p}\overset{¨}{p}}}} & (1.52) \\{= {{\frac{\partial{\,^{2}\phi}}{\partial p^{2}}\;\overset{.}{p}\;\overset{.}{p}} + {J\;\overset{.}{v}}}} & (1.53) \\{= {{J\;\overset{.}{v}} - c}} & (1.54)\end{matrix}$

$\frac{\partial{\,^{2}\phi}}{\partial p^{2}}$is a tensor quantity because φ is a vector.

Note that during simulation, J is the matrix that we actually calculate,not J_(p). The constraint force f_(c) is computed asf_(c)=J^(T)λ  (1.55)where λ is a vector of Lagrange multipliers. This ensures that theminimal amount of work is done to enforce the constraints. Compute f_(c)such that φ(p);J{dot over (v)}=c  (1.56)JM ⁻¹(f _(c) +J ^(T)λ+f_(r))=c  (1.57)JM ⁻¹ J ^(T) λ=c−JM ⁻¹⁽ f _(e) +f _(r))  (1.58)which gives λ, from which f_(c) and {dot over (v)} can be computed. Notethat we can get the same result from solving

$\begin{matrix}{{\begin{bmatrix}{MJ}^{T} \\{J\; 0}\end{bmatrix}\begin{bmatrix}\overset{.}{v} \\\lambda\end{bmatrix}} = \begin{bmatrix}{f_{e} + f_{r}} \\c\end{bmatrix}} & (1.59)\end{matrix}$1.5.2 Implicit EulerThe implicit Euler integration step isp*=p+hQ(p*)v*  (1.60)v*=v+hM ⁻¹(p*)(f _(e)(p*,v*)+(f _(c)(p*,v*)+f _(r)(p*,v*))  (1.61)where the dependence of the right hand sides on the new state is madeobvious. Performing this fully implicit update is rather difficult, evenwhen using low order Rosenbrock methods, for reasons explained later.1.5.3 Velocity Projection MethodInstead of trying to satisfy the acceleration constraint (φ=0) lets tryto satisfy the velocity constraint (φ=0). From equation 1.50:φ(p)=Jv=0  (1.62)Substitute the Euler velocity update (equation 1.43) into this, andsolve for λ;J(v+hM ⁻¹(f _(e) +f _(c) +f _(r)))=0  (1.62)

$\begin{matrix}{{\left( {{JM}^{- 1}\mspace{14mu} J^{T}} \right)\;\lambda} = {c - {\frac{Jv}{h}\;{JM}^{- 1}\;\left( {f_{e} + f_{r}} \right)}}} & (1.63)\end{matrix}$which is similar to equation 1.58 except that the term −Jv/h hasreplaced c. f_(c) and the new state are computed as before. Thisequation ensures that the velocity constraints are met at the new state,which reduces constraint violation compared to equation 1.58. It alsomeans we do not have to have a separate impulse applicationstep—collision constraints for example will automatically result in azero penetration velocity.A further advantage is that the value c does not have to be computed foreach constraint. Although this thing is relatively inexpensive tocompute, it is annoying to derive.1.5.4 Position Projection as a Separate StepExtra work must be done in the velocity projection method to ensure thatthe constraints don't come apart, because although the velocityconstraints are guaranteed to be satisfied at the new state, theposition constraints may not be. One option is Baumgarte stabilization.Another is direct position projection. We start withφ₀ +J _(P)(p−p ₀)=0  (1.65)where φ₀ and J_(p) are re-evaluated at the new position. J_(p) isusually not square so there are many possible p that satisfy thisequation. We want to find the solution which minimizes |p−p₀|. This isdone by settingP−P ₀ =J _(p) ^(T)δ  (1.66)so thatφ₀ +J _(p) J _(p) ^(T)δ=0  (1.67)δ=−(J _(p) J _(p) ^(T−1)φ₀)  (1.68)p←p ₀−(J _(p) J _(p) ^(T−1)φ₀)  (1.69)This corresponds to one iteration of a Newton method to find the root ofφ(p)=(0). Multiple iterations can be performed to get perfectprojection, but this will usually not be necessary.1.5.5 Position Projection MethodWe can take this process one step further by trying to satisfy theposition constraint (φ(p)=(0) at the new timestep. To do this we mustfirst express the Euler position update in terms of the new velocityrather than the current one, otherwise the new position will beindependent of λ.v*=v+hM ⁻¹(f _(e) +J ^(T) λ+f _(r))  (1.70)p*=p+hQv*  (1.71)so thatp*=p+hQ(v+hM ⁻¹(f _(e) +J ^(T) λ+f _(r))  (1.72)The constraint equation we want to satisfy is (to first order)φ_(o) +J _(p)(p*−p)=0  (1.73)But actually to gain more flexibility we will introduce a parameter γwhich controls how much we want to satisfy this constraint, so:φ₀ +J _(p)(p*−p)=(1−γ)φ₀  (1.74)so if γ=0 we are assumed to already have the correct position and noposition projection will be done (γ=1) will give us normal positionprojection). Soφ₀ +J _(p) hQ(v+hM ⁻¹(f _(e) +J ^(T) λ+f _(r)))=(1−γ)φ₀  (1.75)h ²(J _(p) QM ⁻¹ J ^(T))λ=−γφ₀ +hJ _(p) Q(v+hM ⁻¹(f _(e) +f_(r)))  (1.76)

$\begin{matrix}{{\left( {{JM}^{- 1}\mspace{14mu} J^{T}} \right)\;\lambda} = {{- \frac{\gamma\;\phi_{0}}{h^{2}}} - \frac{Jv}{h} - \;{{JM}^{- 1}\;\left( {f_{e} + f_{r}} \right)}}} & (1.77)\end{matrix}$which is similar to equation 1.64 except for the addition of the term−γφ₀/h². This equation ensures that the position constraints are met atthe new state, which reduces constraint violation compared to equation1.64. It also means that a separate position projection step is notrequired (position projection here is considerably less expensive).1.5.6 SummaryFor a standard Euler update, satisfy acceleration constraints by(JM ⁻¹ J ^(T))λ=c−JM ⁻¹(f _(e) +f _(r))  (1.78)For a standard Euler update, satisfy velocity constraints by

$\begin{matrix}{{\left( {{JM}^{- 1}\mspace{14mu} J^{T}} \right)\;\lambda} = {{- \frac{Jv}{h}} - \;{{JM}^{- 1}\;\left( {f_{e} + f_{r}} \right)}}} & (1.79)\end{matrix}$For a modified Euler update, satisfy position constraints by

$\begin{matrix}{{\left( {{JM}^{- 1}\mspace{14mu} J^{T}} \right)\;\lambda} = {{- \frac{\gamma\;\phi^{0}}{h^{2}}} - \frac{Jv}{h} - \;{{JM}^{- 1}\;\left( {f_{e} + f_{r}} \right)}}} & (1.80)\end{matrix}$

2.0 Contact with Friction

The Coulomb friction model relates the normal and tangential force ateach contact point between two bodies. The distinction is made betweenstatic and dynamic friction. With static friction there is no relativemovement (sliding) between points that are in contact, and the followingequation is satisfied:|λ_(t)|≦μλ_(n)  (2.1)where λ_(n) is the (scalar) normal force, λ₁, is the 2×I tangentialforce, and μ is the static friction coefficient. With dynamic frictionthe two surfaces are in relative motion and a different equationapplies:

$\begin{matrix}{\lambda_{1} = {\mu_{d}\;\lambda_{n}\;\frac{v}{n}}} & (2.2)\end{matrix}$where v is the relative surface velocity vector (2×1) and μ_(d) is thedynamic friction coefficient. Equation 2.1 defines a friction cone, theboundary between static and dynamic friction.With acceleration based friction models such as those used by Baraff thedistinction between static and sliding friction is made explicitly, andcare must be taken to ensure that switching between the two modeshappens correctly. Baraff's acceleration based friction model generatesan LCP problem that can sometimes be inconsistent, and even when it isconsistent it is not guaranteed to be solvable by his Cottle-Dantzigbased LCP method.2.1 Velocity Based ModelThe velocity based formulation of Anitescu and Potra resolves some ofthese problems. First, to allow a linear complementarity solution thefriction cone is approximated by a friction pyramid (FIG. 8). Thepyramid has s sides which are given by the 2×1 vectors c₁ . . . c_(s).The conditions corresponding to equation 2.1 are:∀i: c _(i) ^(T)(λ₁−μλ_(n) c _(i))≦0  (2.3)c _(i) ^(T)λ₁−μλ_(n) c _(i) ^(T) c _(i)≦0  (2.4)−c _(i) ^(T)λ₁+μλ_(n)≧0  (2.5)Each side of the friction pyramid has a scalar velocity residual α₁.When the contact is sliding in a direction corresponding to side i,α_(i) is positive. When the contact is within the friction pyramid, allthe α_(i) are 0. The equation of motion is

${{J_{t}v} + {\sum\limits_{i = 1}^{s}{c_{i}\alpha_{i}}}} = 0$where J₁ extracts from the system velocity vector the 2×1 velocityvector in the contact plane. The full system for m contact points is (asa tableau):

$\begin{matrix}{\begin{matrix}{v\;\lambda_{j}} & {\lambda_{t\; 1}\mspace{14mu}\ldots\mspace{14mu}\lambda_{tm}} & {\lambda_{{t\; 1}\mspace{14mu}}\ldots\mspace{14mu}\lambda_{tm}} & {\alpha_{11\mspace{14mu}}\ldots\mspace{14mu}\alpha_{1s}} & {\alpha_{m\; 1}\mspace{14mu}\ldots\mspace{14mu}\alpha_{ms}} \\{M - {J_{j}^{T}J_{j}}} & {{{- J_{{t\; 1}\mspace{14mu}}^{T}}\ldots} - J_{tm}^{T}} & {{{- J_{n\; 1}^{T}}\mspace{14mu}\ldots} - J_{nm}^{T}} & \; & \; \\{J_{t\; 1}\mspace{14mu}\ldots\mspace{14mu} J_{tm}} & \; & \; & {c_{1}\mspace{14mu}\ldots\mspace{14mu} c_{s}} & {c_{1\mspace{14mu}}\ldots\mspace{14mu} c_{s}} \\{J_{{n\; 1}\mspace{14mu}}\ldots\mspace{14mu} J_{nm}} & \; & \; & \; & \; \\\; & {{{- c_{1}^{T\mspace{14mu}}}\ldots} - c_{s}^{T}} & {\mu\mspace{14mu}\ldots\mspace{14mu}\mu} & \; & \; \\\; & {{{- c_{1}^{T}}\mspace{14mu}\ldots} - c_{s}^{T}} & {\mu\mspace{14mu}\ldots\mspace{14mu}\mu} & \; & \;\end{matrix}\begin{matrix} = \\{= {{f_{e} + f_{r}} = 0}} \\{= {{0\mspace{14mu}\ldots} = 0}} \\{\geq {0\mspace{14mu}\ldots} > 0} \\{\geq {0\mspace{14mu}\ldots} > 0} \\{\geq {0\mspace{14mu}\ldots} > 0}\end{matrix}} & (2.6)\end{matrix}$where J_(j) is the joint Jacobian, J_(u) is the tangential velocityJacobian for contact i and J_(ni) is the normal velocity Jacobian forcontact i. In matrix notation,

$\begin{matrix}{{H\begin{bmatrix}v \\\lambda \\\alpha\end{bmatrix}} = q} & (2.7)\end{matrix}$Note that the friction model is anisotropic, and that there is noseparate value of μ for static and dynamic friction.2.1.1 How to Implement this

Directly implementing an LCP solver for the above model is wasteful, asthe matrix that needs to be factorized is larger than necessary (thereare 3+s rows for every contact point). A method that uses only 3 rowsper contact point will be described below. Consider the statement ofstandard LCP:Ax=b+w  (2.8)x≧0  (2.9)w≧0  (2.10)x_(i)w_(i)=0  (2.11)The matrix A has size n×n. The Murty principal pivoting algorithm tosolve this problem goes like this

-   -   Set α={1, 2, . . . , n}. Here α is the set of indexes of        “clamped” variables.    -   Loop        -   Solve for x: x _(a) ,

${= {{AA}\;\frac{- 1}{\overset{\_}{a}\;\overset{\_}{a}}\; b_{\overset{\_}{a}}}},{x_{\overset{\_}{a}} = 0}$

-   -   -   Find the residual w: w=Ax−b.        -   If any elements of x are less than −ε, find the first one            and add its index to α. Restart the loop.        -   If any elements of w are less than −ε, find the first one            and remove its index from α. Restart the loop.        -   If we get to this point, we have a valid solution (x, w).            This procedure is guaranteed to terminate if A is a P            matrix. The value ε is a small tolerance, typically 10⁻⁶. A            variant of this procedure that is more computationally            efficient in practical situations finds the value of x at            each iteration by projecting from the totally unclamped            solution A⁻¹b. At each iteration we ask for the equation            Qx=0 to be true, where Q is a matrix with a row for every            index in α, with a single 1 in the position corresponding to            the index (this is equivalent to asking for all the x_(α),            to be zero).            The projection is done like this:            define W_(α) such that w=W_(α)w_(α)  (2.12)            x=A ⁻¹ b+A ⁻¹ W _(α) w _(α)  (2.13)            QA ⁻¹ b+QA ⁻¹ W _(α) w _(α)=0 (as Qx=0)  (2.14)            w _(α)=−(QA ⁻¹ W _(α))⁻¹ QA ⁻¹ b  (2.15)            w _(α)=−(QA ⁻¹ W _(α))⁻¹ Qx ₀  (2.16)            w_(ā)=0  (2.17)            where            x ₀ =A ⁻¹ b  (2.18)            The value of w_(α) is substituted back to get x.            Now, in our case some of the variables in r correspond to            normal forces (λ_(n)) and some variables correspond to            tangential forces (λ_(tx), λ_(ty)). If the normal force            conditions are violated (λ_(n)≦0) then the normal force can            be projected back to zero as above. If the tangential force            conditions are violated, the tangential forces must be            projected back to the planes that make up the sides of the            friction cone. This can be done by simply altering Q so that            the appropriate plane equations appear on the rows of Q            corresponding to the projected tangential variables.            This is easiest to achieve when we have a four sided            friction pyramid. because then λ_(tx) and λ_(ty) are            effectively decoupled from each other. Also this means that            a tangential variable will be projected to at most one plane            at a time (if we had a many-sided friction volume then many            planes could be involved in a projection and the situation            becomes more complicated).            Here is the modified Murty algorithm:

    -   Make the matrices Q^(n), Q^(h) and Q₁ of size n×n such that        Q^(n)x=0 is the condition for all normal forces to be zero,        Q^(h)x=0 is the condition for all tangential forces to be        clamped at the “high” plane, and Q¹x=0 is the condition for all        tangential forces to be clamped at the “low” plane.

    -   Set α_(n)=α_(h)=α₁={ }. Here α_(n) is the set of clamped normal        forces, α₁ and α_(h) are the sets of clamped-low and        clamped-high tangential indexes.

    -   Loop        -   Set α=α_(n)∩α₁∩α_(h),        -   Set Q:

$\begin{matrix}{Q = \begin{bmatrix}Q_{\alpha_{n}\;\alpha_{n}}^{n} \\Q_{\alpha_{1}\;\alpha_{1}}^{1} \\Q_{\alpha_{h}\;\alpha_{h}}^{h}\end{bmatrix}} & (2.19)\end{matrix}$

-   -   Solve for x:        w _(α)=−(QA ⁻¹ W _(α))⁻¹ Qx ₀  (2.20)        w_(α)=0  (2.21)        x=x ₀ +A ⁻¹ W _(α) w _(α)  (2.22)        -   If any normal elements of x are less than −ε, find the first            one and add its index to α_(n). Restart the loop.        -   If any elements of Q¹ _(x) are less than −ε, find the first            one and add its index to α₁. Restart the loop.        -   If any elements of Q^(h) _(x) are less than −ε, find the            first one and add its index to α_(h). Restart the loop.        -   If any normal elements of w are less than −ε, find the first            one and remove its index from α_(n). Restart the loop.        -   If any clamped-low tangential elements of w are less than            −ε, find the first one and remove its index from α_(ι).            Restart the loop.        -   If any clamped-high tangential elements of w are greater            than C, find the first one and remove its index from α_(h).            Restart the loop.        -   If we get to this point, we have a valid solution (x, w).            This algorithm simulates what happens when the full problem            in the previous section's tableau is solved with the            standard Murty algorithm. This problem is guaranteed to have            at least one solution, but unfortunately this algorithm is            not guaranteed to find it. As μ gets larger the algorithm            will start to cycle though the same index sets. In practice            this rarely happens with physically realistic values of μ.            There are two possible ways to fix this problem. The first            is to try and add heuristics to the projection process to            restrict the possible combinations of clamped and unclamped            states (see the matlab code for some examples). This has not            been successful so far. The second is to detect when we come            back on a previously encountered index set and then alter            the rules for index swapping to prevent us following the            same path. This could possibly be done preemptively at the            level of individual friction pyramids before the global            index set has repeated. This has not been tried yet, it is            difficult in matlab.            There are several simple things that improve the speed of            this algorithm. The first is finding a good starting index            set, by applying the switching condition to every single            index in one go. In fact this procedure may be followed for            several iterations to improve performance, although it must            be eventually abandoned for the one-at-a-time approach to            ensure convergence. Also re-using index sets from the            previous simulation iteration is useful. We need to            investigate heuristics for choosing the switching index. For            example, should we switch on the most or least violating            index?            2.1.2 Tyres            Tyres have special friction requirements. When the tyre is            under longitudinal force, the friction cone is actually            scaled along one axis into a sort of friction ellipse. This            can be simulated by altering the friction pyramid so that it            is a kind of friction “diamond”, and ensuring that the            tangential x and y axes are aligned with the wheel. Tyres            also have the characteristics of slip and pneumatic trail,            which I have not investigated yet.

3.0 More Implicit Integration

It is possible to perform implicit integration on only some statevariables in a system. For example, we can divide up the variables likethis:y^(E)=explicitly integrated variables  (3.1)y^(I)=implicitly integrated variables  (3.2)y ^(E) =f ^(E)(y ^(E) ,y ^(I))  (3.3)y ^(I) =f ^(I)(y ^(E) ,y ^(I))  (3.4)And we can integrate like this (Euler first order):y _(i+1) ^(E) =y _(i) ^(E) +hf ^(E)(y _(i) ^(E) ,y _(i) ¹)  (3.5)y _(i+) ₁ ¹ =y _(i) ¹ +hf ¹(y_(i+1) ^(E) ,y _(i+1) ¹)  (3.6)

$\begin{matrix}{\approx {y_{i}^{1} + {{h\left\lbrack {1 - {h\;\frac{\partial\;}{\partial\,_{y}}\; f^{1}\;(y)}} \middle| {}_{({y_{i}^{E},y_{i}^{l}})} - \right\rbrack}^{- 1}f^{1}\;\left( {y_{i}^{E},y_{i}^{1}} \right)}}} & (3.7)\end{matrix}$More useful in rigid body dynamics is being implicit in some inputs to asystem. Consider the system:{dot over (y)}=f(y,x)  (3.8)x=g(y)  (3.9)Here f is the rigid body system, y is its state variables, and x is the“external” forces presented to it. We can do implicit Euler integrationonly in the variables x as follows:y _(i+1) =y _(i) +hf(y _(i) ,x _(i+1))  (3.10)x _(i+1) =g(y _(i+1))  (3.11)if we make first order approximations for f and g:f(y,x)≈f(y _(i) ,x _(i))+J _(F)(x−x _(i))  (3.12)g(y)≈g(y _(i))+J _(G)(y−y _(i))  (3.13)which gives

$\begin{matrix}{y_{i + 1} = {y_{i} + {\left( {\frac{1}{h} - {J_{F}\; J_{G}}} \right)^{- 1}\; f\;\left( {y_{i},x_{i}} \right)}}} & (3.14)\end{matrix}$If the external forces x are stiff, e.g. if they come from stiffsprings, then this formulation will add extra stability to the systemwithout the overhead of making the rigid body dynamics fully implicit.To implement this scheme we need to compute J_(F) and J_(G). ComputingJ_(G) should be relatively easy. To compute J_(F) first define theexternal force f_(c) asf_(c)=Qx  (3.15)then

$\begin{matrix}{{\frac{\mathbb{d}\;}{\mathbb{d}x}\; f\;\left( . \right)} = {\frac{\mathbb{d}\;}{\mathbb{d}x}\;\overset{.}{v}}} & (3.16) \\{= {\left( {{\frac{\partial\;}{\partial f_{c}}\;\overset{.}{v}} + {\frac{\partial\;}{\partial\lambda}\;{\overset{.}{v} \cdot \frac{\partial\lambda}{\partial f_{c}}}}} \right)\frac{\partial f_{c}}{\partial x}}} & (3.17) \\{= {{M^{- 1}\; Q} - {M^{- 1}\; J^{T}\;\left( {{JM}^{- 1}\; J^{T}} \right)^{- 1}\;{JM}^{- 1}\; Q}}} & (3.18)\end{matrix}$which means that for the already factored system matrix (JM⁻¹J^(T))⁻¹ wemust solve for n_(x) extra right hand sides in the matrix JM⁻¹ Q andthen do a few other cheaper operations. Thus we should be selectiveabout which external forces get the implicit treatment (stiff forces arethe obvious candidates).3.1 Modular SystemsThe construction of modular systems and the application of the chainrule to propagate jacobian information for the purposes of implicitintegration are well known in the art. An example is provided in theMATLAB reference manual.

APPENDIX 2 Contents

1. Conventions

2. Kea core

-   -   2.1. Error Handling    -   2.2. Math stuff        -   2.2.1. Miscellaneous        -   2.2.2. Vector and quaternion stuff        -   2.2.3. Simple Matrix Stuff    -   2.3. Rigid body dynamics core        -   2.3.1. Rigid Body Structure        -   2.3.2. Rigid Body Functions        -   2.3.3. Abstract Constraint Functions        -   2.3.4. Worlds            3. Constraints    -   3.1. Contact    -   3.2. Ball-and-socket    -   3.3. Hinge    -   3.4. Prismatic    -   3.5. Steering hinge (1)        4. Collision        5. Utilities    -   5.1. Kinematics        -   5.1.1. Telescope Segment        -   5.1.2. Keep Place            6. Other    -   6.1. Constraints    -   6.2. Dynamics scheduler    -   6.3. More Functions    -   6.4. Controller layer        1. Conventions        All matrices are stored in column-major order, i.e. stored by        columns.        2. Kea Core

2.1. Error Handling

The following three functions are used internally within Kea to generateerror and warning messages:

-   -   void keaFatalError (char @msg, . . . );    -   void keaDebug (char *msg, . . . );    -   void keaWarning (char *msg, . . . );        Each of these functions has an interface identical to printf (        ), they take an error message string and a variable number of        arguments. These functions can also be called by the user.        The default behavior of these functions is as follows:    -   keaFatalError: print the error message to the console and exit.    -   keaDebug: print the error message to the console, generate        debugging information (e.g. dump core on a unix system) and        exit.    -   keaWarning: print the error message to the console, and continue        running.        The default behavior can be overridden using these functions:    -   typedef void keaErrorFunction (char *msg, . . . );    -   void keaSetFatalErrorHandler (keaErrorFunction *fn);    -   void keaSetDebugHandler (keaErrorFunction *fn);    -   void keaSetWarningHandler (keaErrorFunction *fn);        It is useful to override the default behavior on systems without        a text console (for example the PlayStation 2) or in shipping        software. Note that the fatal error and debug calls are expected        never to return, however they may perform exception handling        using the C set-jmp ( )/longjmp ( )functions.        2.2. Math Stuff        The typedef keaFloat is used everywhere in Kea as the floating        point type. The constant keaInfinity is defined to correspond to        the system infinity value, or if that does not exist the largest        representable floating point value.        2.2.1. Miscellaneous        void keaSetZero (int n, keaFloat *A);    -   Set the first n elements of A to zero. For large arrays this        will normally be faster than a simple for-loop.        2.2.2. Vector and Quaternion Stuff        keaFloat keaDot (keaFloat b [3], keaFloat c [3]);    -   Return the inner product of b and c, assuming both are vectors        of size 3.        void keaCross (keaFloat b [3], keyFloat c [3], keaFloat a [3]);    -   Set a to the cross product of b and c. assuming both are vectors        of size 3.        void keaPlaneSpace (keaFloat n[3], keaFloat a[3], keaFloat b        [3]);    -   Make unit length 3×1 vectors a and b such that together with the        unit length 3×1 vector n they form an orthonormal basis. a and b        span the plane that is normal to n, and a×b n. note that if n is        not normalized then b will not be normalized either.        void keaQProduct (keaFloat p [4], keaFloat q [4], keaFloat r        [4]);    -   Multiplication of quaternions: set r=p*q.        void keaMakeUnitVector (keaFloat v [3]);    -   Make the given size 3 vector unit length.        2.2.3. Simple Matrix Stuff        void keaMultiply (int p, int q, int r, keaFloat *B, keaFloat *C,        keaFloat *A);    -   Set A=B*C, where A is p*r, B is p*q, C is q*r.        void keaMultiplyT1 (int p, int c, int r, keaFloat *B, keaFloat        *C, keaFloat *A);    -   Set A=BT*C, where A is p*r, B is q*p, C is q*r.        2.3. Rigid Body Dynamics Core        2.3.1. Rigid Body Structure        The keaBody structure represents a rigid body in Kea. The        coordinates of a rigid body (x, y, z) are always with respect to        the body's center of mass.        There are a number of internal variables that are made public        for ease of access. You should not modify these directly!        struct keaBody (    -   void *userdata;    -   keaFloat mass, I [9];    -   keaFloat pos [3], qrot [4], vel [6];    -   keaFloat R[9];    -   . . . other internal stuff . . .        )        The body's mass parameters are mass and I, a 3×3 symmetric        inertia tensor matrix. The body's state variables are    -   pos, the center of mass (COM) position (x, y, z).    -   qrot, the four quaternion rotation numbers.    -   vel, the COM velocity (vx, vy, vz). and the angular velocity        (wx, wy, wz).        R is the body 3×3 rotation matrix. It is a direct function of        the qrot vector, and it is updated whenever qrot changes.        userdata is a variable that the user is free to change, this is        never used by Kea.        2.3.2. Rigid Body Functions        All rigid body functions can be called at any time between        simulation timesteps. void keaBodyInitialize (keaBody *body);    -   Initialize the body. This must be the first function called on a        new body.        void keaBodyAttach (keaBody *body, keaWorld *world);    -   Attach the body to the world, making it an active part of that        world. A body must be attached to a world before it can have        constraints attached to it. If the body is already attached to        another world it will be detached from that world first. If the        body is already attached to the world then nothing will be done.        void keaBodyDetach (keaBody *body);    -   Detach the body from whatever world it is currently attached to.        Any constraints that are connected to this body are disconnected        first. This does not destroy any body data, it simply prevents        the body from being a part of the simulation. The body can be        reattached at any time.        Now here are some functions to set the mass distribution of the        body.        void keaBodyMakeSphere (keaBody *body, keaFloat mass, keaFloat        radius);    -   Set the mass parameters of this body to a sphere of the given        mass and radius.        void keaBodyMakeBox (keaBody *body, keaFloat mass, keaFloat 1x,        keaFloat 1y, keaFloat 1z);    -   Set the mass parameters of this body to a box of the given mass        and side lengths.        Now here are some functions to set the position, rotation, and        velocity of the body. If you set values that are inconsistent        with the current constraints then the simulation will attempt to        correct this in subsequent time steps.        void keaBodvSetPosition (keaBody *body, keaFloat x, keaFloat y,        keaFloat z);        void keaBodySetQuaternion (keaBody *body, keaFloat q1, keaFloat        q2, keaFloat q3, keaFloat q4);        void keaBodySetLinearVelocity (keaBody *body, keaFloat dx,        keaFloat dy, keaFloat dz);        void keaBodySetAngularVelocity (keaBody *body, keaFloat wx,        keaFloat wy, keaFloat wz);        Now here are some functions to add forces to the body. After        each time step the body is assumed to have zero force acting on        it. These functions accumulate force on the body for the next        time step.        void keaBodyAndForceAbs (keaBody *body, keaFloat fx, keaFloat        fy, keaFloat fz);        void keaBodyAddForceRel (keaBody *body, keaFloat fx, keaFloat        fy, keaFloat fz);    -   Add a force, in the absolute (inertial) frame or the relative        (body) frame, to the body's center of mass.        void keaBodyAddTorqueAbs    -   (keaBody *body, keaFloat tx, keaFloat ty, keaFloat tz);        void keaBodyAddTorqueRel    -   (keaBody *body, keaFloat tx, keaFloat ty, keaFloat tz);    -   Add a torque, in the absolute (inertial) frame or the relative        (body) frame, to the body's center of mass.        2.3.3. Abstract Constraint Functions        The keaConstraint structure represents a one, two or three body        constraint. The constraint services described below are used by        all the system joint types, and allow new constraint types to be        created by the user. Note that all variables in the        keaConstraint structure are internal and should not be accessed        directly.        The following constraint functions can be called at any time in        the simulation.        void keaConstraintInitialize (keaConstraint *c);    -   Initialize the constraint. This must be the first function        called on a new constraint.        void keaConstraintAttach (keaConstraint *c, keaWorld *world);    -   Attach the constraint to the given world, making it an active        part of that world. If the constraint is attached to another        world, it will be detached from that world first. If the        constraint is already attached to the world then nothing will be        done.        void keaConstraintDetach (keaConstraint *c);    -   Detach the constraint from whatever world it is currently        attached to. This does not destroy any constraint data, it        simply prevents the constraint from being a part of the        simulation. The constraint can be re-attached at any time.        void keaConstraintSetBodies (keaConstraint *c, keaBody *b1,        keaBody *b2, keaBody *b3);    -   This set the bodies that the constraint attaches. The constraint        must have been attached to a world, and the bodies must be in        the same world.        keaConstraintDefine (keaConstraint *c, int num_ce, keaGetInfoFn        *getinfo, keaStartStepFn    -   *startstep);    -   Sets the behavior of the constraint. This is only called to        define new constraint types. num_ce is the number of constraint        equations. getinfo and startstep are pointers to functions that        implement the constraint behavior. getinfo gets information        about this constraint for the current state of the constrained        bodies. startstep is called automatically at the start of each        timestep, it can set some auxiliary state-based data (such as        joint angles) which the user can read. If you change the state        of the constraint or the bodies which it connects then you may        call this function yourself to update that data. Arguments to        the getinfo function are provided in a structure rather than        being passed directly, to allow for future expansion without        having to rewrite all the constraint code. The getinfo function        is free to ignore any of the arguments in this structure, except        for the essential ‘J’. Each matrix/vector here has num_ce rows        to fill in.        struct keaConstraintInfo (    -   keaFloat *J[3];    -   int rowskip;    -   keaFloat *c;    -   keaFloat *xi;    -   keaFloat *lower, *upper;    -   keaFloat *slipfactor;        );        typedef void keaGetInfoFn (keaConstraintInfo *);        The structure members are:    -   J: Up to 3 pointers to Jacobian matrices that must be filled in.        These matrices are stored by rows for convenience in constraint        formation, in contrast to the usual Kea storage convention.    -   rowskip: How much to jump by to go between J matrix rows.    -   c: Vector in the constraint equation J*v=c.    -   xi: Constraint error vector.    -   lower, upper: Lagrange multiplier limits.    -   slipfactor: First order constraint slipping vector.        2.3.4. Worlds        The keaWorld structure represents a simulated world in Kea. All        members of this structure are internal and should not be        accessed directly.        void keaWorldInitialize (keaWorld *world);    -   Initialize a new world. After initialization, bodies and        constraints can be attached to it.        void keaWorldDestroy (keaWorld *world);    -   Destroy the given world. This simply detaches all bodies and        constraints from the world, emptying it.        void keaWorldAddGravity (keaWorld *world, keaFloat gravity);    -   Add a downwards (−z) gravity force to all bodies in the world.        gravity is given in m/s2.        void keaWorldStep1 (keaWorld *world, keaFloat stepsize);    -   Evolve the world forward in time by stepsize seconds. This uses        an algorithm which will be fast, except for systems containing        large articulated rigid body structures.        void keaWorldSetEpsilon (keaWorld *world, keaFloat x);        void keaWorldSetGamma (keaWorld *world, keaFloat x);    -   These functions set world parameters. Increasing epsilon helps        to combat numerical instability problems caused by degenerate        systems. Increasing it will make the simulation more        “non-physical” but may smooth over simulation glitches. The        default value is 0.0001, increasing this to 0.1-1 will result in        observable non-physical effects for worlds where that masses are        on the order of 1 kg.    -   Gamma is the projection constant which controls constraint        stabilization. If the rigid body configuration has diverged from        its constrained configuration, the next time step it will be        brought a fraction ‘gamma’ of the way back to its correct        configuration. Setting gamma to zero will result in articulated        structures gradually coming apart. Setting gamma to one and        higher will result in instabilities as the simulation tries to        “overcorrect”. The default value is 0.2, and that is probably        good enough for most simulations.        3. Constraints        To make a constraint, call its initialization functions, and        then call the keaConstraintAttach ( ) and keaConstraintSetBodies        ( ) functions.        3.1. Contact        The keaContact constraint is a collision contact between body 1        and body 2, or a collision contact between body 1 and the static        environment. The keaContact structure has a number of public        variables that must be set before the world step function is        called:        struct keaContact (    -   int mode;    -   keaFloat cpos [3];    -   keaFloat normal [3];    -   keaFloat penetration;    -   keaFloat maxforce;    -   keaFloat a [3];    -   keaFloat k1;    -   . . . other internal stuff.        )        The fields of keaContact are:    -   mode: The contact mode is one of the following constants:        -   o: Zero friction.        -   KEA_FRICTION-2D: Friction in two directions using            automatically determined principal directions.        -   KEA_FRICTION_(—)1D: Friction in one direction (vector a).        -   KEA_FRICTION_TYRE1: Friction in wheel drive direction            (vector a) and first order slip in the lateral (other            tangential) direction. The slip factor is the value k1. The            following flags can be ORed with mode:        -   KEA_FRICTION_BOX: Friction force magnitude along each            principal direction is limited to maxforce.    -   cpos: The contact position, in absolute coordinates. This must        always be set.    -   normal: The vector that is normal to the contact sliding plane,        relative to body 1. This must have unit length. This must point        ‘in’ to body 1, that is body 1 motion is allowed along the        direction of +normal and body 2 motion is allowed along the        direction of −normal. This must always be set.    -   penetration: The penetration distances of the contact along the        normal direction. This must always be set.    -   max_force: The maximum frictional force that can be applied.        This is set when the box friction flag is set.    -   a: Parameter vector a. This is set in KEA_FRICTION_(—)1D and        KEA_FRICTION_TYRE1 modes. It must have unit length.    -   k1: Parameter k1. This is set in KEA_FRICTION_TYRE1 mode.        The keaContact functions are:        void keaContactInitialize (keaContact *contact);    -   Initialize a new contact.        3.2. Ball-and-Socket        The keaBSJoint structure represents a ball and socket joint.        void keaBSJointhnitialize (keaBSJoint *joint);    -   Initialize a new ball and socket joint.        keaBSJointSetPosition (keaBSJoint *joint, keaFloat x, keaFloat        y, keaFloat z);    -   Set the joint position (in absolute coordinates). The constraint        bodies must have been set first. and the positions of the joined        bodies must have been set.        3.3. Hinge        The keaHinge structure represents a hinge joint. Note that the        initial position of the hinge will be taken as the zero        reference for angle determination.        void keaHingeInitialize (keaHinge *joint);    -   Initialize a new hinge joint.        void keaHingeSetPosition    -   (keaHinge *joint, keaFloat x, keaFloat y, keaFloat z);        void keaHingeSetAxis    -   (keaHinge *joint, keaFloat x, keaFloat y, keaFloat z);    -   Set the position of the hinge joint and its axis (in absolute        coordinates). The joint bodies must have been set first, and the        positions of the joined bodies must have been set.        void keaHingeSetNoLimits (keaHinge *joint);        void keaHingeSetLimits (keaHinge *joint, keaFloat low, keaFloat        high);    -   Set joint limits. low and high are in radians and are relative        to the zero reference determined by the initial position of the        hinge.        void keaHingeSetNoMotor (keaHinge *joint);        void keaHingeSetLimitedForceMotor (keaHinge *joint, keaFloat        desired_velocity, keaFloat force limit);    -   Sets a motor on the hinge.        keaFloat keaHingeGetAxisAbs (keaHinge *joint);    -   Returns a pointer to a size 3 vector which gives the current        hinge axis in absolute coordinates. keaFloat keaHingeGetAngle        (keaHinge *joint);        keaFloat keaHingeGetAngleRate (keaHinge *joint);    -   Returns the current hinge angle and angular velocity.        3.4. Prismatic        The keaPrism structure represents a prismatic joint.        void keaprismInitialize (keaprism *joint);    -   Initialize a new prismatic joint.        void keaPrismSetAxis (keaprism *joint, keaFloat x, keaFloat y,        keaFloat z);    -   Set the sliding axis for the prismatic joint (in absolute        coordinates). The joint bodies must have been set first, and the        positions of the joined bodies must have been set.        void keaPrismSetNoLimits (keaprism *joint);        void keaPrismSetLimits (keaprism *joint, keaFloat low, keaFloat        high);    -   Set joint limits. low and high are in meters-position zero is        when the centers of mass of the two bodies are as close to each        other as possible.        void keaPrismSetNoMotor (keaprism *joint);        void keaPrismSetLimitedForceMotor (keaPrism *joint, keaFloat        desired_velocity, keaFloat force limit);    -   Sets a motor on the joint.        keaFloat *kea PrismGetAxisAbs (keaprism *joint);    -   Returns a pointer to a size 3 vector which gives the current        sliding axis in absolute coordinates.        keaFloat keaPrismGetPosition (keaprism *joint);        keaFloat keaPrismGetPositionRate (keaprism *joint);    -   Returns the current sliding position and velocity. Position zero        is when the centers of mass of the two bodies are as close to        each other as possible.        3.5. Steering Hinge (1)        The keaSteeringHinge structure represents a steering hinge        joint, which is a hinge that can be steered along a steering        axis. This joint is useful on the front wheels of cars. Body 1        is the chassis and body 2 is the wheel. The connection point for        the wheel body is its center of mass.        void keaSteeringHingeInitialize (keaSteeringHinge *joint);    -   Initialize a new steering hinge joint.        void keaSteeringHingeSetSteeringAxis    -   (keaSteeringHinge *joint, keaFloat x, keaFloat y, keaFloat z)        void keaSteeringHingeSetHingeAxis    -   (keaSteeringHinge *joint, keaFloat x, keaFloat y, keaFloat z);    -   These functions set the joint geometry, the steering axis and        the hinge axis. The joint bodies must have been set first, and        the positions of the joined bodies must have been set.        keaFloat *keaSteeringHingeGetHingeAxisAbs (keaSteeringHinge        *joint);        keaFloat *keaSteeringHingeGetSteeringAxisAbs (keaSteeringHinge        *joint);    -   Returns pointers to size 3 vectors which give the current hinge        and steering axes in absolute coordinates.        4. Collision        The Kea collision API is in a state of flux and will not be        documented here yet. But check out the source file kea_collide.        h if you want to know what the current story is.        5. Utilities        5.1. Kinematics        These functions allow for easy kinematic (rather than dynamic)        placement of objects. They are specific to particular kinematic        situations that the author has come across in the past, so not        all common cases are covered here!        5.1.1. Telescope Segment        The keaKinematicTelescopeSegment structure and associated        functions allow kinematic placement of an intermediate telescope        segment (body 3) given the positions of the segments at the ends        of the telescope (body I and body 2).        void keaKineTaticTelescopeSegmentInitialize        (keaKinematicTelescopeSegment *k,    -   keaFloat pos1 [3], keaFloat R [9], keaFloat pos2 [3], keaFloat        pos3 [3]);    -   Initialize a keaKinematicTelescopeSegment structure, giving the        initial positions of the three bodies, and the rotation matrix        of body 1.        void keaKinematicTelescopeSegmentGetPosition        (keaKinematicTelescopeSegment *k, keaFloat pos1 [3], keaFloat R        [9], keaFloa pos2 [3], keaFloat pos3 [3]);    -   Given the positions of bodies 1 and the rotation matrix of body        1, return the position of body 3.        5.1.2. Keep Place        The keaKinematicKeepPlace structure and associated functions        allow kinematic placement of a body (number 2) always in the        same place relative to another body (number 1).        void keaKinematicKeepDlaceInitialize (keaKinematicKeepPlace *k,        keaFloat pos1 [3], keaFloat R [9], keaFloat pos2 [3]);    -   Initialize a keaKinematicKeepPlace structure, giving the initial        positions of the bodies, and the rotation matrix of body 1.        void keaKinematicKeepPlaceGetPosition (keaKinematicKeepPlace *k,        keaFloat pos1 [3], keaFloat R [9], keaFloat pos2 [3]);    -   Given the position and rotation matrix of body 1, return the        position of body 2.        6. Other        Some parts of the Kea API are not covered here, mostly those        parts that haven't even been designed yet! Here are notes about        what is missing.        6.1. Constraints        More constraint types are needed, especially the linear-1,        angular-1 stuff from the SDK.        This will be trivial to add.        Document the functions that help implement user defined        constraints, e.g. getting the bodies a constraint attaches.        For the contact constraint, we are currently missing:    -   velocity dependent slip.    -   the friction cone one-step-late approximation.    -   keep the physical stuff in separate structure, i.e. separate the        physical quantities from the geometrical ones.        Take a look at how well the keaConstraint functions operate on        the joint types—is there an annoying type casting issue?        6.2. Dynamics Scheduler        This things hasn't even been designed yet. It could sit outside        the Kea core, in which case we must check how to detach groups        of RBs and constraints from the world without destroying the        relationships between then, so they can be attached again later.        6.3. More Functions        Need more functions to compute rotations, e.g. to set body        orientation. Use functions from glowworm. Open source.        Need more functions to add force to bodies, e.g. at a non-COM        position.        Again, use the functions from glowworm. Open source.        6.4. Controller Layer        An open source dataflow based control layer, that allows us to        easily implement ‘gadgets’ such as springs, PD controllers etc.        Issues: data transfer (wiring), encapsulation of basic Kea        structures, driving of the simulation. This should be quite        easy.

APPENDIX 3 Stiff Spring Simulation

From Notes by Russell Smith

This describes how to implement a stiff spring in Kea using constraints.The key is to allow first order slip along the spring direction.Consider a one dimensional single particle system (mass m):{dot over (p)}=v  (3.1)

$\begin{matrix}{\overset{.}{v} = {\frac{l}{m}\left( {f + {J^{T}\lambda}} \right)}} & (3.2)\end{matrix}$where p is the point position, v is its velocity, f is the externalforce and λ is the constraint force (all scalars). We will enforce theconstraint p=0 by setting J to 1. Thus from equation 1.80 in appendix 1,λ is computed as:

$\begin{matrix}{{\left( {\frac{l}{m} + ɛ} \right)\lambda} = {{- \frac{\gamma\; p}{h^{2}}} - \frac{v}{h} - \frac{f}{m}}} & (3.3)\end{matrix}$so the semi-implicit update for p and v is:

$\begin{matrix}{v_{i + 1} = {v_{i} + {\frac{h}{m}\left( {f + \lambda} \right)}}} & (3.4) \\{= {v_{i} + \frac{hf}{m} + \frac{{- \frac{\gamma\; p}{hm}} - \frac{v}{m} - \frac{hf}{m^{2}}}{\frac{1}{m} + ɛ}}} & (3.5) \\{= {v_{i} + {{hf}\left( \frac{ɛ}{1 + {m\; ɛ}} \right)} + \frac{{- \frac{\gamma\; p}{h}} - v}{1 + {m\; ɛ}}}} & (3.6) \\{= {v_{i} + {f\left( \frac{h\; ɛ}{1 + {m\; ɛ}} \right)} + {v\left( \frac{m\; ɛ}{1 + {m\; ɛ}} \right)} + {p\left( \frac{- \gamma}{h\left( {1 + {m\; ɛ}} \right)} \right)}}} & (3.7)\end{matrix}$andp _(i+1) =p _(i) +hv _(i+1)  (3.8)Now consider removing the constraint, adding an external spring anddamper force, and integrating implicitly:

$\begin{matrix}{f_{i} = {{{- k_{p}}p_{i}} - {k_{d}v_{i}}}} & (3.9) \\{\lambda = 0} & (3.10) \\{p_{i + 1} = {p_{i} + {hv}_{i + 1}}} & (3.11) \\{v_{i + 1} = {v_{i} + {\frac{h}{m}f_{i + 1}}}} & (3.12) \\{= {v_{i} + {\frac{h}{m}\left( {{{- k_{p}}p_{i + 1}} - {k_{d}v_{i + 1}}} \right)}}} & (3.13) \\{= {v_{i} + {\frac{h}{m}\left( {{- {k_{p}\left( {p_{i} + {hv}_{i + 1}} \right)}} - {k_{d}v_{i + 1}}} \right)}}} & (3.14) \\{{v_{i + 1}\left( {1 + \frac{{h^{2}k_{p}} + {hk}_{d}}{m}} \right)} = {v_{i} - {\frac{{hk}_{p}}{m}p_{i}}}} & (3.15) \\{V_{i + 1} = {{v_{1}\left( \frac{m}{m + {h^{2}k_{p}} + {hk}_{d}} \right)} + {p_{i}\left( \frac{- {hk}_{p}}{m + {h^{2}k_{p}} + {hk}_{d}} \right)}}} & (3.16)\end{matrix}$We can equate coefficients with equation (3.7) to see how to constructan “implicitly integrated spring” with a constraint (take f to be zeroin equation (3.7)). First equate the coefficient on v_(j) to find ε:

$\begin{matrix}{\frac{m\; ɛ}{1 + {m\; ɛ}} = \frac{m}{m + {h^{2\;}k_{p}} + {hk}_{d}}} & (3.17) \\{{ɛ\left( {m + {h^{2}k_{p}} + {hk}_{d}} \right)} = {1 + {m\; ɛ}}} & (3.18) \\{ɛ = \frac{1}{{h^{2}k_{p}} + {hk}_{d}}} & (3.19)\end{matrix}$Then equate the coefficient on p_(i) to find γ:

$\begin{matrix}{\frac{- \gamma}{h\left( {1 + {m\; ɛ}} \right)} = {\frac{- {hk}_{p}}{m + {h^{2}k_{p}} + {hk}_{d}}`}} & (3.20) \\{\gamma = \frac{h^{2}{k_{p}\left( {1 + {m\; ɛ}} \right)}}{m + {h^{2}k_{p}} + {hk}_{d}}} & (3.21) \\{= \frac{{hk}_{p}}{{hk}_{p} + k_{d}}} & (3.22)\end{matrix}$The parameters ε and γ are both dependent on the time step, but they arenot dependent on the mass, so this constraint functions as a truespring. When ε is 0 this corresponds to a spring or damper constant ofinfinity, which results in an unmovable spring (complete constraintsatisfaction).

APPENDIX 4 4.1 Linear Complementarity Problems

Given a real square n×n matrix M and a real n-dimensional vector q, thecomplementarity problem consists of finding the n-dimensional vectors zand w such that they satisfy the following conditions:Mz+q=w  (4.26)z_(i)≧0 ∀iε(1, 2, . . . , n)  (4.27)w_(i)≧0∀iε(1, 2, . . . , n)  (4.28)w_(i)z_(i)≧0 ∀iε(1, 2, . . . , n)  (4.29)This is fundamentally a combinatorial problem and various directalgorithms are available where the search goes through index sets α, βsuch that α⊂{1, 2, . . . , n}, β⊂{1, 2, . . . , n), α∩β=Ø, α∪β={1, 2, .. . , n}. The set α is the set of active variables and w_(i)=0 ∀iεαwhile the set β is the set of free variables such that z_(i)=0 ∀iεβ. Theproblem is then partitioned as:

$\begin{matrix}{{{\begin{bmatrix}{M_{\alpha\alpha}M_{\alpha\beta}} \\{M_{\beta\alpha}M_{\beta\beta}}\end{bmatrix}\begin{bmatrix}z_{\alpha} \\0\end{bmatrix}} + \begin{bmatrix}q_{\alpha} \\q_{\beta}\end{bmatrix}} = \begin{bmatrix}0 \\w_{\beta}\end{bmatrix}} & (4.30)\end{matrix}$where the subscripts α, β, are used to specify all indices that are inthe sets α, β respectively. This is equivalent to the linear algebraproblem:M _(αα) z _(α) =−q  (4.31)w _(β) =M _(βα) z _(α) +q _(β)  (4.32)which must be solved for z_(α) while B is computed by directsubstitution. The matrix M_(αα) is known as a principal submatrix of thematrix M. Various principal pivoting algorithms perform the same basiccomputation but take different strategies to revise the sets α and βfrom the computed complementarity point which is a vector s such that:

$\begin{matrix}{{si} = \left\{ \begin{matrix}z_{i} & {\forall{i\;\varepsilon\;\alpha}} \\w_{i} & {\forall{i\;{\varepsilon\beta}}}\end{matrix} \right.} & (4.33)\end{matrix}$All principal pivot methods go through a sequence of sets α_(i), β_(i),i=1, 2, . . . until a solution is found i.e., s_(i)≧0 ∀iε{1, 2, . . . ,n}.4.2 The Murty AlgorithmOne important such algorithm is the Murty Principal Pivot Method whichis as follows:

1. Initialize (basically, any guess for α⁽⁰⁾ is good), set i=0.

2. form principal submatrix M_(α) ^((i)) _(α) ^((i)) and solveMα^((i))α^((i))z_(α) ^((i))=q_(α) ^((i))

3. compute s^((i))=z_(α) ^((i))+w_(β) ^((i)) where w_(β) ^((i))=M_(β)^((i)) _(α) ^((i))z_(α) ^((i))

4. if s_(j) ^((i))≧0 ∀ j we are done

-   -   else        -   find the smallest j such that s_(j) ^((i))<0,        -   if s_(j) ^((i))=z_(j) then α^((i+1))=α^((i))\{j}        -   else α^((i+1))=α^((i))∪{j}        -   i←i+1        -   goto step 2            This algorithm is special in that it is stateless and can be            started from any initial guess for α₀. This method will work            on any P matrix and in particular, any positive definite            matrix.            The flowchart for this algorithm is given in FIG. I below.            4.3 The Kostreva Perturbation Method            In the case where the matrix M is positive semi-definite,            the Murty principal pivot method can fail. This can arise in            a multi-body simulation in the case where the constraints            are degenerate, or if we work from the optimization matrix            which leads to the form:

$\begin{matrix}{{{\begin{bmatrix}{N - J^{T}} \\{J\mspace{31mu} 0}\end{bmatrix}\begin{bmatrix}v \\\lambda\end{bmatrix}} + \begin{bmatrix}{- F} \\c\end{bmatrix}} = \begin{bmatrix}0 \\v\end{bmatrix}} & (4.34)\end{matrix}$λ≧0 v≧0  (4.35)λ¹v=0  (4.36)for a multibody problem with inequality constraints. This matrix ispositive semi-definite if the mass submatrix N is positive definite(which is always the case for physical systems). However, given anynumber ε>0, the matrix obtained from the original one by adding ε on thediagonal is always positive definite, i.e., the matrix:

$\begin{matrix}{M_{ɛ} = \begin{bmatrix}{N + {ɛ\; l_{N}}} & {- J^{T}} \\J & {ɛ\; l_{J}}\end{bmatrix}} & (4.37)\end{matrix}$is positive definite, where 1_(N) and i_(J) are identity matrices ofappropriate sizes. Kostreva demonstrated that one can solve the sequenceof complementarity problems defined by a sequence of positive numbers{_(n)}_(n=1) ^(∞) such that ε→0 as n→∞ and find an answer to thepositive semi-definite problem or find that it is infeasible (i.e.,there is no answer). The algorithm is as follows;

1. Choose ε⁽⁰⁾>0 (typically 10⁻⁶)

2. Solve LCP(M_(ε) ⁽⁰⁾, q)

3. set i←i+1. choose ε^((i))<ε^((i−1))

4. Solve LCP(M_(ε) ^((i)), q)

5. if ∥z^((i))−z^((i−1))∥< to 1 Solve LCP(M,q) and finish

6. else if ∥z^((i))−z^((i−1))∥<max goto step 2

7. else error: problem is infeasible

We often set ε⁽⁰⁾=10⁻⁶ and ε⁽¹⁾=0. This is often sufficient. In Kea, weeven go further and set ε⁽⁰⁾=10⁻³ and stop and away i.e., we don'tbother don't perturbation.

4.4 Boxed LCPs

The boxed LCP problem starts from the definition of the standard LCP andadds two new n-dimensional vectors 1 and u, lower and upper boundsrespectively, such that 1_(i)<u_(i) ∀Iε{1, 2, . . . , n} and then theproblem reads:Mz+q=w ₊ −w  (4.38)z _(i) −l _(i)≧0 ∀iε{1, 2, . . . , n}  (4.39)w _(+i)≧0 ∀iε{1, 2, . . . , n}  (4.40)(z _(i) −l _(i))w _(+i)=0 ∀iε{1, 2, . . . , n}  (4.41)u _(i) −z _(i)≧0 ∀iε{1, 2, . . . , n}  (4.42)w _(−i)≧0 ∀iε{1, 2, . . . , n}  (4.43)(u _(i) −z _(i))w _(+i)=0 ∀iε{1, 2, . . . , n}  (4.44)This is equivalent to a much larger mixed linear complementarity problemdefined as:

$\begin{matrix}{{{\begin{bmatrix}M & {- {ll}} \\l & {0\mspace{11mu} 0}\end{bmatrix}\begin{bmatrix}z \\w_{+} \\w_{-}\end{bmatrix}} + \begin{bmatrix}q \\{- l} \\u\end{bmatrix}} = \begin{bmatrix}0 \\\mu \\v\end{bmatrix}} & (4.21)\end{matrix}$μ_(i) ,w _(+i)≧0 μ_(i) w _(+i)0 v _(i) ,w _(−i)≧0 v _(i) w_(−i)=0  (4.22)

This is precisely the sort of problem that the Kostreva procedure isdesigned to solve. However, because of the structure of the problem,there are simplifications that can be achieved on this. The idea is topartition the set 3 defined as above into two sets, γ and t so thatz_(j)=1_(j) for jεy and Z_(j)=u_(j) for jεt. Afterwards, we follow theMurty algorithm but we change the definition of the complementaritypoint s as follows:

$\begin{matrix}{s_{j} = \left\{ \begin{matrix}{\min\left( {{z_{j} - 1_{j}},{u_{j} - z_{j}}} \right)} & {\forall{j\;{\varepsilon\alpha}}} \\\left( {{{M_{\gamma\alpha}z_{\alpha}} - {M_{\gamma\gamma}I_{\gamma}} - M_{\gamma 1}},{u_{1} + q_{\gamma}}} \right)_{j} & {\forall{j\;{\varepsilon\gamma}}} \\{- \left( {{M_{1\alpha}z_{\alpha}} - {M_{1\gamma}l_{\gamma}} - {M_{\alpha}u_{1}} + q_{1}} \right)_{j}} & {\forall{j\;\varepsilon\mspace{11mu}\iota}}\end{matrix} \right.} & (4.23)\end{matrix}$and then, the least index rule is applied to this new complementaritypoint as follows:j=min arg(s _(j)<0)if jεα^((i)) and z_(j) ^((i))<1_(j) thenα^((i+1))=α^((i))\{j}γ^((i+1))=γ^((i))∪{j}, ι^((i+1))=ι^((i))if jεα^((i)) and z_(j) ^((i)))>μ_(j) then α^((i+1))=α^((i))/{j},ι^((i+1))=ι^((i) ∪{j}, γ) ^((i+1))=γ^(i)if jεγ^((i)) then α^((i+1))=α^((i))∪{j}, γ^((i+1))=γ^((i))\{j},ι^((i+1))=ι^((i))if jεγ^((i)) then α^((i+1))=α^((i))∪{j}, t^((i+1))=t(i)/{j},γ^((i+1))=γ^((i))This modification of the standard Murty algorithm has not been traced inthe literature by the authors yet.4.5 Box FrictionThe Coulomb friction model specifies the tangential forces at a point ofcontact in terms of the normal force at that point. The model specifiesnon-ideal constraint forces i.e., forces arising from constraints thatdo work on the system. This is in sharp contrast to typical idealconstraints which do no work on the system. Coulomb friction implementsa maximum dissipation principle i.e., when the velocity at the point ofcontact is non-zero, the tangential force will be aligned against thevelocity in a way that maximizes the work done; for isotropic friction,this means that the force of friction is directly opposed to thevelocity of the contact point in the contact plane. One should note thatCoulomb friction is a constitutive law i.e., an empirical model which ismeant to summarize experimental evidence, in contrast with a fundamentallaw which can be derived from first principles. As such, “Coulomb's Law”is not so strict: the modeler has license to alter this model to easecomputation provided the overall behaviour is still close to what isseen in an experiment.The Coulomb friction model for a single point describes two states forthe contact namely, sticking or sliding. In stick mode or staticfriction mode, the tangential force vector f₁, which lies in a planetangential to the normal force of contact, the force that prevents twoobjects from interpenetrating, must have smaller magnitude than thefriction coefficient μ_(s) times the magnitude of the normal force f_(n)i.e.,∥f _(t)∥=√{square root over (f _(t1) ² +f _(t2) ²)}≦μ_(s) f _(n)  (4.24)where f_(t1) and f_(t2) are components of the tangential force along twoperpendicular directions in the tangential plane. In dynamic or slidingfriction, the friction force must oppose the sliding velocity vector v₁and its magnitude is the kinetic friction coefficient μ_(k) times thenormal force i.e.∥f _(t)∥=μ_(kfn)  (4.25)f _(t) ·v _(t) =f _(t1) v _(t1) +f _(t2) v _(t2)≦0  (4.26)This model does not specify how to compute f_(n) or f_(t) at all butonly states relationships between those quantities. These conditionsspecify that the tangential force should lie in the convex cone definedby the normal force, i.e.,C _((fn)) ={f _(n) +f _(t) | ∥f _(t)∥<μ_(s) f _(n)}  (4.27)The first approximation we perform on this is to replace the frictioncone by a friction pyramid, a simple case of polygonal coneapproximation found in the literature. The idea is to introduce klinearly dependent basis vectors for the contact plane denoted d₁, d₂, .. . , and to represent the tangential friction force as:

$\begin{matrix}{f_{t} = {\sum\limits_{i}{d_{i}\beta_{i}}}} & (4.28)\end{matrix}$and from this definition, we get an approximation to the friction coneknown as the friction polyhedron:

$\begin{matrix}{C_{({fn})} = \left\{ {{f_{n} + {\sum\limits_{i}{d_{i}\beta_{i}}}}❘{0 < \beta_{i} < {\mu_{s}{f_{n}}}}} \right\}} & (4.29)\end{matrix}$The construction is shown for the friction pyramid in FIG. 4 using fourbasis vectors with equal angular spacing.The final approximation is to neglect the dependence of the tangentialfriction forces on the normal force by specifying an independent maximumvalue for the coefficients, f_(max). This gives a simple approximationoff the friction cone that we refer to as a ‘box friction’ model.

$\begin{matrix}{B_{({fn})} = \left\{ {f_{n} + {\sum\limits_{i}{d_{i}\beta}}} \middle| {0 < \beta_{i} < f_{\max}} \right\}} & (4.30)\end{matrix}$A box friction approximation to the friction cone is shown in FIG. 5.

1. A computer-implemented method, using a computer having at least aprocessing unit and a memory, for determining motion of simulatedobjects each of which is represented by a data structure indicatingcharacteristics of the simulated object, wherein each of at least someof the simulated objects have visual characteristics and simulatedphysical characteristics and wherein the motion of the simulated objectsis to be in compliance with a physics model and is to be represented ona display device such that a viewer perceives the motion of thesimulated objects in compliance with the physics model, the methodcomprising: storing, in the memory, position parameters and velocityparameters, the position and velocity parameters defining an initialstate of the physics model and the simulated objects; storing, in thememory, constraint parameters defining at least one constraint functionconstraining the motion of the simulated objects according to thephysics model, wherein the constraint parameters include at least oneposition constraint constraining the motion of the plurality of rigidbodies; calculating, using the processor, the position and velocityparameters defining the state of the simulated objects after apredetermined time step based on rigid body dynamics, including: a)carrying out a semi-implicit integration step subject to the constraintsconstraining the motion of the simulated objects, to determine thevelocity after the step; and b) determining constraint forces that actto keep the simulated objects in compliance with the constraints byensuring that the first derivative of the constraint function is zero;generating, using the processor, at the end of the predetermined timestep, a screen image of a series of screen images for display on thedisplay device, the screen image displaying a view of the simulatedobjects such that the viewer can perceive the motion of the simulatedobjects in compliance with the physics model, the screen image beingbased on the calculated position and velocity parameters defining thestate of the simulated objects at the end of the predetermined timestep; and displaying the screen image on the display device; wherein thephysics model includes a model of friction in which the frictional forcebetween a pair of objects is independent of a normal force between theobjects for both elastic and inelastic collisions.
 2. The method ofclaim 1, further comprising before the step of calculating in theprocessor the position and velocity parameters, a step of: storing inthe memory parameters defining a bounded constraint force to simulatethe effects of friction in which the constraint force acts in the planeof contact between a pair of objects to prevent sliding of one of thepair of objects over the other of the pair, wherein the constraint forceis bounded to be not greater than a predetermined constant value toallow sliding of the objects over one another and thus include dynamicfriction in the simulation.
 3. A method according to claim 1 wherein thecalculating step includes carrying out the implicit integration by:calculating the velocity parameters after the time step from theexternal forces, the constraint forces and the position and velocityparameters before the time step, and calculating the position parametersafter the time step from the external forces and constraint forces, thecalculated velocity parameters after the time step and the positionparameters before the time step.
 4. A method according to claim 1wherein the frictional force between a pair of objects is modelled as abounded constraint force in which the constraint force acts in the planeof contact between the pair of objects to prevent sliding of one of thepair of objects over the other of the pair, wherein the constraint forceis bounded to be not greater than a predetermined constant value toallow sliding of the objects over one another and thus include dynamicfriction in the simulation.
 5. A method according to claim 1 wherein thebounds on the constraint forces are included by a step of testingwhether the constraint forces have a magnitude greater than apredetermined value and if so setting them to be that predeterminedvalue.
 6. A method of generating a display in a computer game accordingto claim
 1. 7. A method according to claim 1, wherein the constraintforces are determined by solving the mixed linear complementarityproblem using a Murty algorithm.
 8. A method according to claim 5wherein the linear complementarity problem is solved by solving theboxed LCP problem by the boxed Murty's method.
 9. A method according toclaim 7 wherein the constraints are required to be held to within apredetermined tolerance ε.
 10. A method according to claim 9 where ε hasa value between 10⁻⁴ and 10⁻².
 11. A method according to claim 1, inwhich the constraints are modelled using the first order expansion ofthe constraint function Φ, such that Φ=φ_(o)+J_(p)(p′−p)=(1−γ)φ_(o),where γ is a relaxation parameter, φ_(o) is a constraint, J_(p) is theJacobian of the constraint forces based on position, p′ is a positionparameter after the predetermined timestep, and p is a positionparameter before the predetermined timestep.
 12. A method according toclaim 11 wherein γ and a predetermined tolerance ε on the constraintsare chosen to model a compliant coupling between two of the rigidbodies.
 13. A computer readable medium storing a plurality of programcode for controlling a data processor of a computer system, that whenexecuted, cause the data processor to perform one or more tasks forsimulating the motion of objects and displaying the results on a displayscreen, the computer system including a memory having memory locationsfor position parameters and for velocity parameters, the position andvelocity parameters defining an initial state of a model system having aplurality of bodies, the plurality program code comprising: program codefor storing, in the memory, constraint parameters defining at least oneconstraint function constraining the motion of the bodies in the modelsystem, wherein the constraint parameters include at least one positionconstraint constraining the motion of the plurality of rigid bodies;program code for calculating the position and velocity parametersdefining the state of the system after a predetermined time step basedon rigid body dynamics, including: a) program code for performing asemi-implicit integration step subject to the constraints constrainingthe motion of the simulated objects to determine the velocity after thestep; and b) program code for determining constraint forces that act tokeep the system in compliance with the constraints by ensuring that thefirst derivative of the constraint function is zero; and program codefor generating, at the end of the predetermined time step, a screenimage of a series of screen images for display on the display screen,the screen image displaying a view of the simulated objects such thatthe viewer can perceive the motion of the simulated objects incompliance with the physics model, the screen image being based on thecalculated position and velocity parameters defining the state of thesimulated objects at the end of the predetermined time step; and programcode for displaying the screen image on the display screen, wherein themodel system includes a model of friction in which the frictional forcebetween a pair of objects is independent of a normal force between theobjects for both elastic and inelastic collisions.
 14. The computerreadable medium of claim 13, wherein the data carrier is a cartridge fora computer game machine.
 15. The computer readable medium of claim 13,wherein the semi-implicit integration step comprises: program code forcalculating the velocity parameters after the time step from externalforces, the constraint forces and the position and velocity parametersbefore the time step, and program code for calculating the positionparameters after the time step from the external forces and constraintforces, the calculated velocity parameters after the time step and theposition parameters before the time step each according to theconstraints.
 16. Apparatus for simulating the motion of objects anddisplaying the results on a display screen comprising: a display screen,a memory, a processing unit, and a computer program stored in the memoryfor causing the apparatus to carry out the steps of: a) storing in thememory position and velocity parameters defining an initial state of amodel system having a plurality of bodies, b) storing in the memoryparameters defining at least one constraint function constraining themotion of the bodies in the model system, wherein the at least oneconstraint function defines at least one position constraintconstraining the motion of the plurality of rigid bodies, c) storing inthe memory parameters defining a bounded constraint force to simulatethe effects of friction in which the constraint force acts in the planeof contact between a pair of objects to prevent sliding of one of thepair of objects over the other of the pair, wherein the constraint forceis bounded to be not greater than a predetermined constant value toallow sliding of the objects over one another and thus include dynamicfriction in the simulation, and wherein the bounded constraint force isindependent of a normal force between the objects for both elastic andinelastic collisions; d) calculating in the processor the position andvelocity parameters defining the state of the system after apredetermined time step based on rigid body dynamics, including i)carrying out a semi-implicit integration step subject to theconstraints, to determine the velocity after the step; and ii)determining the constraint forces that act to keep the system incompliance with the constraints by ensuring that the first derivative ofthe constraint function is zero; and e) generating, at the end of thepredetermined time step, a screen image of a series of screen images fordisplay on the display screen, the screen image displaying a view of thesimulated objects such that the viewer can perceive the motion of thesimulated objects in compliance with the physics model, the screen imagebeing based on the calculated position and velocity parameters definingthe state of the simulated objects at the end of the predetermined timestep, and f) displaying, on the display screen, the screen image.
 17. Acomputer-implemented method, using a computer, for simulating movementof a plurality of rigid bodies according to a physics model bycalculating motion of the plurality of the rigid bodies the methodcomprising: storing, into storage accessible to the computer, positionparameters defining initial positions of a plurality of rigid bodies;defining at least one position constraint constraining the motion of theplurality of rigid bodies, the at least one position constraint definingconstraints on positions of the plurality of rigid bodies represented bythe position parameters read from the storage; providing the at leastone position constraint to the computer; determining a boundedconstraint force, using the computer, wherein the bounded constraintforce represents a frictional force between a pair of objects of theplurality of rigid bodies, the frictional force acting in a plane ofcontact between the pair of objects and wherein at least one reactionforce is bounded to be not greater than a predetermined value relativeto the frictional force, and wherein the bounded constraint force isindependent of a normal force between the objects for both elastic andinelastic collisions; calculating, using the computer, new positionparameters for the plurality of rigid bodies using the computer, the newposition parameters defining new positions of the plurality of rigidbodies after a predetermined time step, wherein the new positionparameters depend on at least one reaction force that acts to keep theplurality of rigid bodies in compliance with the at least one positionconstraint, such that the new position parameters satisfy the at leastone position constraint directly without an iterative error process; andgenerating, using the processor, at the end of the predetermined timestep, a screen image of a series of screen images for display on adisplay device, the screen image displaying a view of the simulatedobjects such that the viewer can perceive the motion of the simulatedobjects in compliance with the physics model, the screen image beingbased on the calculated position and velocity parameters defining thestate of the simulated objects at the end of the predetermined timestep; and displaying the screen image on the display device.
 18. Themethod according to claim 17, further comprising constraining at leastone reaction force to not exceed a predetermined value by setting the atleast one reaction force equal to the predetermined value if it wouldexceed the predetermined value.
 19. The method according to claim 17,further comprising displaying an image of the plurality of rigid bodiesat their calculated positions on a display.
 20. The method according toclaim 17, further comprising: storing, into the storage, velocityparameters defining initial velocities of the plurality of rigid bodies;defining at least one combined constraint constraining the motion of theplurality of rigid bodies, the at least one combined constraint definingconstraints on velocities and positions of the plurality of rigid bodiesrepresented by the velocity parameters and position parameters read fromthe storage; providing the at least one combined constraint to thecomputer, and wherein calculating the new position parameters depends onat least one reaction force that acts to keep the plurality of rigidbodies in compliance with the at least one position constraint and theat least one combined constraint.
 21. The method according to claim 20,further comprising storing, into the storage, force parameters definingat least one initial force acting on the plurality of rigid bodies, andwherein the at least one constraint further includes constraints on thepositions of the plurality of rigid bodies that depend upon the storedposition parameters, the velocity parameters and the force parametersread from the storage.
 22. The method according to claim 20, furthercomprising calculating new velocity parameters for the plurality ofrigid bodies defining new velocities of the plurality of rigid bodiesafter the predetermined time step.
 23. A computer system comprising aprocessor and instruction memory, the instruction memory comprisinginstructions that, when executed, calculate simulated movement of aplurality of rigid bodies according to a physics model from storedrepresentations of the plurality of rigid bodies, the computer systemcomprising: a data memory configured to store position parametersdefining initial positions of the plurality of rigid bodies; aconstraint definer that, when executed, defines at least one positionconstraint constraining motion of the plurality of rigid bodies, the atleast one position constraint defining constraints on positions of theplurality of rigid bodies represented by the stored position parametersread from the storage; a friction modeler operable to determine abounded constraint force, wherein the bounded constraint forcerepresents a frictional force between a pair of objects of the pluralityof rigid bodies, the frictional force acting in a plane of contactbetween the pair of objects, wherein at least one reaction force isbounded to be not greater than a predetermined value relative to thefrictional force, and wherein the bounded constraint force isindependent of a normal force between the objects for both elastic andinelastic collisions; and a position parameter calculator that, whenexecuted, calculates new position parameters for the plurality of rigidbodies defining new positions of the plurality of rigid bodies after apredetermined time step, wherein the new position parameters depend onat least one reaction force that acts to keep the plurality of rigidbodies in compliance with the at least one position constraint, suchthat the new position parameters satisfy the at least one positionconstraint directly without an iterative error process; a imagegenerator that when executed at the end of the predetermined time step,generates a screen image of a series of screen images for display on adisplay device, the screen image displaying a view of the simulatedobjects such that the viewer can perceive the motion of the simulatedobjects in compliance with the physics model, the screen image beingbased at least in part on the position parameters for the plurality ofrigid bodies at the end of the predetermined time step; wherein theprocessor is configured, when executed, to perform processing operationsthat implement the constraint definer and the position parametercalculator in accordance with computer executable instructions stored inthe instruction memory.
 24. Computer system according to claim 23, thecomputer executable instructions stored in the instruction memoryfurther comprising instructions for determining a bounded constraintforce, wherein the bounded constraint force represents a frictionalforce between a pair of objects of the plurality of rigid bodies, thefrictional force acting in a plane of contact between the pair ofobjects and wherein at least one reaction force is bounded to be notgreater than a predetermined value relative to the frictional force. 25.Computer system according to claim 23, the computer executableinstructions stored in the instruction memory further comprisinginstructions for constraining at least one reaction force to not exceeda predetermined value by setting the at least one reaction force equalto the predetermined value if it would exceed the predetermined value.26. Computer system according to claim 23, the computer executableinstructions stored in the instruction memory further comprisinginstructions to generate a display of an image of the plurality of rigidbodies at their calculated positions on a display.
 27. Computer systemaccording to claim 23, wherein the data memory is further configured tostore velocity parameters defining initial velocities of the pluralityof rigid bodies, and wherein the constraint definer configured with theat least one constraint including at least one combined constraintconstraining velocities and positions of the plurality of rigid bodiesrepresented by the velocity parameters and position parameters stored inthe data memory.
 28. Computer system according to claim 27, wherein thedata memory is further configured to store force parameters defining atleast one initial force acting on the plurality of rigid bodies, andwherein the at least one constraint further includes constraints on thepositions of the plurality of rigid bodies that depend upon the storedposition parameters, the velocity parameters and the force parametersread from the data memory.
 29. Computer system according to claim 27,further comprising a velocity parameter calculator configured tocalculate new velocity parameters for the plurality of rigid bodiesafter the predetermined time step.
 30. Computer system according toclaim 23, further comprising a reaction force calculator configured tocalculate at least one reaction force.
 31. Computer system according toclaim 30, wherein the reaction force calculator is configured, whenexecuted, to calculate the at least one reaction force to not exceed apredetermined value by setting the at least one reaction force equal tothe predetermined value if it would exceed the predetermined value. 32.Computer system according to claim 23, the computer executableinstructions stored in the instruction memory further comprisinginstructions for defining at least one combined constraint constrainingthe motion of the plurality of rigid bodies, the at least one combinedconstraint defining constraints on velocities and positions of theplurality of rigid bodies represented by the velocity parameters andposition parameters read from the storage, and for calculating newposition parameters for the plurality of rigid bodies defining newpositions of the plurality of rigid bodies after a predetermined timestep, wherein the new position parameters depend on at least onereaction force that acts to keep the plurality of rigid bodies incompliance with the at least one position constraint and the at leastone combined constraint.
 33. Computer system according to claim 32, thecomputer executable instructions stored in the instruction memoryfurther comprising instructions for storing force parameters defining atleast one initial force acting on the plurality of rigid bodies and forimplementing constraints on the positions of the plurality of rigidbodies that depend upon the stored position parameters, the velocityparameters and the force parameters read from the data memory. 34.Computer system according to claim 32, the computer executableinstructions stored in the instruction memory further comprisinginstructions for calculating new velocity parameters for the pluralityof rigid bodies defining new velocities of the plurality of rigid bodiesafter the predetermined time step.
 35. Processing apparatus forcalculating simulated movement of a plurality of rigid bodies accordingto a physics model, the processing apparatus comprising: means forstoring position parameters defining initial positions of a plurality ofrigid bodies; means for defining at least one position constraintconstraining the motion of the plurality of rigid bodies, the at leastone position constraint defining constraints on positions of theplurality of rigid bodies represented by the position parameters readfrom the means for storing; means for determining a bounded constraintforce, wherein the bounded constraint force represents a frictionalforce between a pair of objects of the plurality of rigid bodies, thefrictional force acting in a plane of contact between the pair ofobjects, wherein at least one reaction force is bounded to be notgreater than a predetermined value relative to the frictional force, andwherein the bounded constraint force is independent of a normal forcebetween the objects for both elastic and inelastic collisions; means forcalculating new position parameters for the plurality of rigid bodiesdefining new positions of the plurality of rigid bodies after apredetermined time step, wherein the new position parameters depend onat least one reaction force that acts to keep the plurality of rigidbodies in compliance with the at least one position constraint, suchthat the new position parameters satisfy the at least one positionconstraint directly without an iterative error process; and means forgenerating, at the end of the predetermined time step, a screen image ofa series of screen images for display on a display device, the screenimage displaying a view of the simulated objects such that the viewercan perceive the motion of the simulated objects in compliance with thephysics model, the screen image being based on the calculated positionand velocity parameters defining the state of the simulated objects atthe end of the predetermined time step; and means for displaying thescreen image on the display device.
 36. A data carrier tangiblyembodying a program of machine readable instructions executable by adigital processing apparatus having stored thereon instructions that,when executed, calculate simulated movement of a plurality of rigidbodies according to a physics model, the program comprising: programcode for storing, into storage accessible to the computer, positionparameters defining initial positions of a plurality of rigid bodies;program code for defining at least one position constraint constrainingthe motion of the plurality of rigid bodies, the at least one positionconstraint defining constraints on positions of the plurality of rigidbodies represented by the position parameters read from the storage;program code for determining a bounded constraint force, wherein thebounded constraint force represents a frictional force between a pair ofobjects of the plurality of rigid bodies, the frictional force acting ina plane of contact between the pair of objects, wherein at least onereaction force is bounded to be not greater than a predetermined valuerelative to the frictional force, and wherein the bounded constraintforce between the pair of objects is independent of a normal forcebetween the objects for both elastic and inelastic collisions; programcode for calculating new position parameters for the plurality of rigidbodies defining new positions of the plurality of rigid bodies after apredetermined time step, wherein the new position parameters depend onat least one reaction force that acts to keep the plurality of rigidbodies in compliance with the at least one position constraint, suchthat the new position parameters satisfy the at least one positionconstraint directly without an iterative error process; and program codefor generating, at the end of the predetermined time step, a screenimage of a series of screen images for display on a display device, thescreen image displaying a view of the simulated objects such that theviewer can perceive the motion of the simulated objects in compliancewith the physics model, the screen image being based on the calculatedposition and velocity parameters defining the state of the simulatedobjects at the end of the predetermined time step; and program code fordisplaying the screen image on the display device.
 37. The data carriertangibly embodying a program of machine readable instructions accordingto claim 36, further comprising program code for constraining at leastone reaction force to not exceed a predetermined value by setting the atleast one reaction force equal to the predetermined value if it wouldexceed the predetermined value.
 38. The data carrier tangibly embodyinga program of machine readable instructions according to claim 36,further comprising program code for displaying an image of the pluralityof rigid bodies at their calculated positions on a display.
 39. The datacarrier tangibly embodying a program of machine readable instructionsaccording to claim 36, further comprising: program code for storing,into the storage, velocity parameters defining initial velocities of theplurality of rigid bodies; program code for defining at least onecombined constraint constraining the motion of the plurality of rigidbodies, the at least one combined constraint defining constraints onvelocities and positions of the plurality of rigid bodies represented bythe velocity parameters and position parameters read from the storage;and program code for calculating new position parameters for theplurality of rigid bodies defining new positions of the plurality ofrigid bodies after a predetermined time step, wherein the new positionparameters depend on at least one reaction force that acts to keep theplurality of rigid bodies in compliance with the at least one positionconstraint and the at least one combined constraint.
 40. The datacarrier tangibly embodying a program of machine readable instructionsaccording to claim 39, further comprising program code for storing, intothe storage, force parameters defining at least one initial force actingon the plurality of rigid bodies, and the at least one constraintfurther includes constraints on the positions of the plurality of rigidbodies that depend upon the stored position parameters, the velocityparameters and the force parameters read from the storage.
 41. The datacarrier tangibly embodying a program of machine readable instructionsaccording to claim 39, further comprising program code for calculatingnew velocity parameters for the plurality of rigid bodies defining newvelocities of the plurality of rigid bodies after the predetermined timestep.